diff --git a/.devcontainer/.gdb/gdbinit b/.devcontainer/.gdb/gdbinit index 443fbab9..39499425 100644 --- a/.devcontainer/.gdb/gdbinit +++ b/.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 diff --git a/.gitignore b/.gitignore index b7cc579a..9e1b48fe 100644 --- a/.gitignore +++ b/.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 diff --git a/Makefile b/Makefile index 55819bb0..79c95f4d 100644 --- a/Makefile +++ b/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 diff --git a/docker/Dockerfile.humble-dev b/docker/Dockerfile.humble-dev index 6a72f1f3..9ff84499 100644 --- a/docker/Dockerfile.humble-dev +++ b/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 diff --git a/docker/Dockerfile.jazzy-dev b/docker/Dockerfile.jazzy-dev index 87f38b72..896e5b51 100644 --- a/docker/Dockerfile.jazzy-dev +++ b/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 diff --git a/docker/Dockerfile.noetic-dev b/docker/Dockerfile.noetic-dev index bbead602..bcbb39d2 100644 --- a/docker/Dockerfile.noetic-dev +++ b/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 diff --git a/docker/ci.sh b/docker/ci.sh index f01035fc..facb015d 100755 --- a/docker/ci.sh +++ b/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} \ diff --git a/docker/scripts/install_apt_base.sh b/docker/scripts/install_apt_base.sh index 674784ca..dcc5fec3 100755 --- a/docker/scripts/install_apt_base.sh +++ b/docker/scripts/install_apt_base.sh @@ -30,6 +30,7 @@ packages=$(awk -v filt=${FPSDK_IMAGE%-*} '$1 ~ filt { print $2 }' < + ${catkin_INCLUDE_DIRS} +) + # EXECUTABLES ========================================================================================================== diff --git a/examples/ros2_fpsdk_demo/CMakeLists.txt b/examples/ros2_fpsdk_demo/CMakeLists.txt index 4b1a8bce..632e4433 100644 --- a/examples/ros2_fpsdk_demo/CMakeLists.txt +++ b/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 + $ +) + install(TARGETS ${PROJECT_NAME}_lib ${PROJECT_NAME}_node LIBRARY DESTINATION lib diff --git a/fpsdk.code-workspace b/fpsdk.code-workspace index 898c037f..c57f0d6a 100644 --- a/fpsdk.code-workspace +++ b/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 }, diff --git a/fpsdk.sh b/fpsdk.sh index 9f1974ad..0b60758d 100755 --- a/fpsdk.sh +++ b/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 diff --git a/fpsdk_apps/CMakeLists.txt b/fpsdk_apps/CMakeLists.txt index 86f99670..98a0c715 100644 --- a/fpsdk_apps/CMakeLists.txt +++ b/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 $<$:fpsdk_ros1> + $<$:fpsdk_ros2> + $ + ${rosbag2_cpp_TARGETS} ) file(GLOB PARSERTOOL_CPP_FILES parsertool/*.cpp) diff --git a/fpsdk_apps/fpltool/fpltool.cpp b/fpsdk_apps/fpltool/fpltool.cpp index fd61164b..8101a903 100644 --- a/fpsdk_apps/fpltool/fpltool.cpp +++ b/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 } diff --git a/fpsdk_apps/fpltool/fpltool_doc.hpp b/fpsdk_apps/fpltool/fpltool_doc.hpp index 7fa5f1a1..a3f14c23 100644 --- a/fpsdk_apps/fpltool/fpltool_doc.hpp +++ b/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 diff --git a/fpsdk_apps/fpltool/fpltool_dump.cpp b/fpsdk_apps/fpltool/fpltool_dump.cpp index abde626d..2f3187ee 100644 --- a/fpsdk_apps/fpltool/fpltool_dump.cpp +++ b/fpsdk_apps/fpltool/fpltool_dump.cpp @@ -95,8 +95,8 @@ bool DoDump(const FplToolOptions& opts) while (!sig_int.ShouldAbort() && reader.Next(log_msg)) { // "extra" levels // - <= 0: report progress while scanning through the log - // - == 1: print message info - // - >= 2: add less or more hexdump + // - == 1: also print message info + // - >= 2: also add (partial) hexdump for each message // Report progress if (opts.progress_ > 0) { @@ -158,7 +158,7 @@ bool DoDump(const FplToolOptions& opts) // Print summary of the message if (opts.extra_ > 0) { - std::printf("message %8" PRIu64 " 0x%08" PRIx64 " 0x%06" PRIx32 " %-15s %s\n", log_msg.file_seq_, + std::printf("data %8" PRIu64 " 0x%08" PRIx64 " 0x%06" PRIx32 " %-15s %s\n", log_msg.file_seq_, log_msg.file_pos_, log_msg.RawSize(), FplTypeStr(log_type), info.c_str()); } diff --git a/fpsdk_apps/fpltool/fpltool_extract.cpp b/fpsdk_apps/fpltool/fpltool_extract.cpp index b93a0b56..c8bfb66a 100644 --- a/fpsdk_apps/fpltool/fpltool_extract.cpp +++ b/fpsdk_apps/fpltool/fpltool_extract.cpp @@ -14,19 +14,30 @@ /* LIBC/STL */ #include #include +#include #include /* EXTERNAL */ +#include /* Fixposition SDK */ #include #include #include #include +#include #include +#include +#include +#if defined(FPSDK_USE_ROS1) +# include +#elif defined(FPSDK_USE_ROS2) +# include +#endif /* PACKAGE */ #include "fpltool_extract.hpp" +#include "fpltool_utils.hpp" namespace fpsdk { namespace apps { @@ -35,8 +46,18 @@ namespace fpltool { using namespace fpsdk::common::app; using namespace fpsdk::common::fpl; -using namespace fpsdk::common::string; using namespace fpsdk::common::path; +using namespace fpsdk::common::ros1; +using namespace fpsdk::common::string; +using namespace fpsdk::common::types; +#ifdef FPSDK_USE_ROS1 +using namespace fpsdk::ros1::bagwriter; +#endif +#ifdef FPSDK_USE_ROS2 +using namespace fpsdk::ros2::bagwriter; +#endif + +// --------------------------------------------------------------------------------------------------------------------- bool DoExtract(const FplToolOptions& opts) { @@ -44,19 +65,9 @@ bool DoExtract(const FplToolOptions& opts) WARNING("Need exactly one input file"); return false; } - const std::string input_fpl = opts.inputs_[0]; - // Determine prefix for output file names - std::string output_prefix; - if (opts.output_.empty()) { - output_prefix = input_fpl; - StrReplace(output_prefix, ".fpl", ""); - if ((opts.skip_ > 0) || (opts.duration_ > 0)) { - output_prefix += "_S" + std::to_string(opts.skip_) + "-D" + std::to_string(opts.duration_); - } - } else { - output_prefix = opts.output_; - } + const std::string input_fpl = opts.inputs_[0]; + const std::string output_prefix = opts.GetOutputPrefix(input_fpl); // Open input log FplFileReader reader; @@ -64,29 +75,74 @@ bool DoExtract(const FplToolOptions& opts) return false; } - // Output files, and a helper func to write to them - std::map> output_files; - auto WriteOutput = [&output_files, &input_fpl, &opts, &output_prefix]( - const std::string& name, const std::vector& data) -> bool { - // Create and open new file if necessary - auto entry = output_files.find(name); - if (entry == output_files.end()) { - entry = output_files.insert({ name, std::make_unique() }).first; - const std::string path = output_prefix + "_" + name + ".raw"; - if (!opts.overwrite_ && PathExists(path)) { - WARNING("Output file %s already exists", path.c_str()); - return false; - } - NOTICE("Extracting %s to %s", input_fpl.c_str(), path.c_str()); - if (!entry->second->Open(path)) { - return false; - } + // Output files + OutputFileHelper output(output_prefix, opts); + const char* JSONL_NAME = (opts.compress_ > 0 ? "all.jsonl.gz" : "all.jsonl"); + const char* RAW_EXT = (opts.compress_ > 0 ? ".raw.gz" : ".raw"); + + // Check which output formats we want + bool doJsonl = opts.formats_.empty(); + bool doRaw = opts.formats_.empty(); + bool doFile = opts.formats_.empty(); +#if defined(FPSDK_USE_ROS1) || defined(FPSDK_USE_ROS2) + bool doRos = opts.formats_.empty(); +#else + bool doRos = false; +#endif + + for (auto& fmt : opts.formats_) { + if (fmt == opts.FORMAT_JSONL) { + doJsonl = true; + } else if (fmt == opts.FORMAT_RAW) { + doRaw = true; + } else if (fmt == opts.FORMAT_FILE) { + doFile = true; + } else if (fmt == opts.FORMAT_ROS) { +#if defined(FPSDK_USE_ROS1) || defined(FPSDK_USE_ROS2) + doRos = true; +#else + WARNING("Cannot extract to ROS bag. This fpltool is not built with ROS support."); + return false; +#endif + } else { + WARNING("Bad argument '%s' to option -e, --formats", fmt.c_str()); + return false; } - if (!entry->second->Write(data)) { + } + + if (!doJsonl && !doRaw && !doFile && !doRos) { + WARNING("No output formats selected"); + return false; + } + + NOTICE("Extracting from %s to %s_...", input_fpl.c_str(), output_prefix.c_str()); + +#if defined(FPSDK_USE_ROS1) || defined(FPSDK_USE_ROS2) + BagWriter bag; +# ifdef FPSDK_USE_ROS1 + const auto output_bag = output_prefix + ".bag"; // File +# else + const auto output_bag = output_prefix + "_bag"; // Directory! (even for single-file .mcap) +# endif + if (PathExists(output_bag)) { + if (!opts.overwrite_) { + WARNING("Output bag %s already exists", output_bag.c_str()); return false; + } else { + RemoveAll(output_bag); } - return true; - }; + } + if (doRos && !bag.Open(output_bag, opts.compress_)) { + return false; + } + + NOTICE("Extracting to %s", output_bag.c_str()); +#endif + + // Helper for converting ROS1 messages to JSON + RosMsgHelper rosmsg_helper; + // Helper for processing STREAMMSGs + ParserMsgHelper parsermsg_helper; // Handle SIGINT (C-c) to abort nicely SigIntHelper sig_int; @@ -96,7 +152,10 @@ bool DoExtract(const FplToolOptions& opts) double progress = 0.0; double rate = 0.0; bool ok = true; + bool have_meta = false; + std::set files_dumped; uint32_t time_into_log = 0; + std::size_t errors = 0; while (!sig_int.ShouldAbort() && reader.Next(log_msg) && ok) { // Report progress if (opts.progress_ > 0) { @@ -117,37 +176,135 @@ bool DoExtract(const FplToolOptions& opts) // Process message const auto log_type = log_msg.PayloadType(); switch (log_type) { + case FplType::LOGSTATUS: { + const LogStatus logstatus(log_msg); + if (logstatus.valid_) { + time_into_log = logstatus.log_duration_; + if (!skip && doJsonl && !output.WriteJson(JSONL_NAME, logstatus)) { + ok = false; + } + } else { + WARNING("Invalid LOGSTATUS"); + errors++; + } + break; + } + + case FplType::LOGMETA: { + const LogMeta logmeta(log_msg); + if (logmeta.valid_) { + if ((!have_meta || !skip) && doJsonl && !output.WriteJson(JSONL_NAME, logmeta)) { + ok = false; + } + have_meta = true; + } else { + WARNING("Invalid LOGMETA"); + errors++; + } + break; + } + case FplType::STREAMMSG: if (!skip) { const StreamMsg streammsg(log_msg); if (streammsg.valid_) { - if (!WriteOutput(streammsg.stream_name_, streammsg.msg_data_)) { + if (doJsonl || doRos) { + parsermsg_helper.UpdateParserMsg(streammsg); + } + if (doRaw && !output.WriteData(streammsg.stream_name_ + RAW_EXT, streammsg.msg_data_)) { + ok = false; + } + if (doJsonl && + !output.WriteStreamMsg(JSONL_NAME, streammsg, parsermsg_helper.GetParserMsg(true))) { ok = false; } +#if defined(FPSDK_USE_ROS1) // || defined(FPSDK_USE_ROS2) // @todo implement for ROS2, s.a. fpltools_utils.hpp + if (doRos) { + bag.WriteMessage(parsermsg_helper.GetRosMsg(), "/" + streammsg.stream_name_ + "/raw", + streammsg.rec_time_); + } +#endif } else { WARNING("Invalid STREAMMSG"); - ok = false; + errors++; } } break; - case FplType::LOGSTATUS: { - const LogStatus logstatus(log_msg); - if (logstatus.valid_) { - time_into_log = logstatus.log_duration_; + + case FplType::ROSMSGDEF: { + RosMsgDef rosmsgdef(log_msg); + if (rosmsgdef.valid_) { + rosmsgdef.topic_name_ = FixTopicName(rosmsgdef.topic_name_); + + if (doJsonl || doRos) { + rosmsg_helper.AddDef(rosmsgdef); + } +#if defined(FPSDK_USE_ROS1) || defined(FPSDK_USE_ROS2) + if (doRos) { + bag.AddMsgDef(rosmsgdef); + } +#endif + } else { + WARNING("Invalid ROSMSGDEF"); + errors++; } break; } - case FplType::FILEDUMP: - case FplType::LOGMETA: - case FplType::ROSMSGDEF: + case FplType::ROSMSGBIN: + if (!skip) { + RosMsgBin rosmsgbin(log_msg); + if (rosmsgbin.valid_) { + rosmsgbin.topic_name_ = FixTopicName(rosmsgbin.topic_name_); + + nlohmann::json jdata; + if (doJsonl && rosmsg_helper.ToJson(rosmsgbin, jdata) && !output.WriteJson(JSONL_NAME, jdata)) { + ok = false; + } +#if defined(FPSDK_USE_ROS1) || defined(FPSDK_USE_ROS2) + if (doRos && !bag.WriteMessage(rosmsgbin)) { + ok = false; + } +#endif + } else { + WARNING("Invalid ROSMSGBIN"); + errors++; + } + } + break; + + case FplType::FILEDUMP: { + FileDump filedump(log_msg); + if (filedump.valid_) { + const bool dumped_before = files_dumped.count(filedump.filename_) == 0; + if ((!dumped_before || !skip) && doFile && + !output.WriteData( + FileDumpOutName(filedump) + (opts.compress_ > 0 ? ".gz" : ""), filedump.data_)) { + ok = false; + } + if ((!dumped_before || !skip) && doJsonl && !output.WriteJson(JSONL_NAME, filedump)) { + ok = false; + } + if (!dumped_before) { + files_dumped.emplace(filedump.filename_); + } + } else { + WARNING("Invalid FILEDUMP"); + errors++; + } + } + case FplType::BLOB: - case FplType::UNSPECIFIED: case FplType::INT_D: case FplType::INT_F: case FplType::INT_X: + case FplType::UNSPECIFIED: break; } + if (errors >= 100) { + WARNING("Too many errors!"); + ok = false; + } } // We were interrupted @@ -156,16 +313,17 @@ bool DoExtract(const FplToolOptions& opts) } // Close output files - for (auto& entry : output_files) { - const std::string path = entry.second->Path(); - entry.second->Close(); - const double raw_size = FileSize(path) / 1024.0 / 1024.0; + output.CloseAll(ok); +#if defined(FPSDK_USE_ROS1) || defined(FPSDK_USE_ROS2) + if (doRos) { + bag.Close(); if (ok) { - INFO("Wrote file %s (%.1f MiB)", path.c_str(), raw_size); + INFO("Wrote bag %s (%s)", output_bag.c_str(), OutputSizeStr(output_bag).c_str()); } else { - WARNING("Incomplete raw %s (%.1f MiB)", path.c_str(), raw_size); + WARNING("Incomplete bag %s (%s)", output_bag.c_str(), OutputSizeStr(output_bag).c_str()); } } +#endif return ok; } diff --git a/fpsdk_apps/fpltool/fpltool_opts.hpp b/fpsdk_apps/fpltool/fpltool_opts.hpp index 1616289c..5baf3b4f 100644 --- a/fpsdk_apps/fpltool/fpltool_opts.hpp +++ b/fpsdk_apps/fpltool/fpltool_opts.hpp @@ -36,13 +36,20 @@ namespace fpltool { /** * @brief Program options */ -class FplToolOptions : public fpsdk::common::app::ProgramOptions +class FplToolOptions : public common::app::ProgramOptions { public: FplToolOptions() // clang-format off : ProgramOptions("fpltool", { - { 'f', false }, { 'o', true }, { 'x', false }, { 'p', false }, - { 'P', false }, { 'c', false }, { 'S', true }, { 'D', true } }) {}; // clang-format on + { 'f', false, "force" }, + { 'o', true, "output" }, + { 'x', false, "extra" }, + { 'p', false, "progress" }, + { 'P', false, "no-progress" }, + { 'c', false, "compress" }, + { 'S', true, "skip" }, + { 'D', true, "duration" }, + { 'e', true, "formats" } }) {}; // clang-format on /** * @brief Commands, modes of operation @@ -69,72 +76,169 @@ class FplToolOptions : public fpsdk::common::app::ProgramOptions int compress_ = 0; //!< Compress output uint32_t skip_ = 0; //!< Skip start [sec] uint32_t duration_ = 0; //!< Duration [sec] + std::vector formats_; //!< List of output formats for extraction // clang-format on + static constexpr const char* FORMAT_JSONL = "jsonl"; + static constexpr const char* FORMAT_RAW = "raw"; + static constexpr const char* FORMAT_FILE = "file"; + static constexpr const char* FORMAT_ROS = "ros"; + void PrintHelp() override final { // clang-format off std::fputs( "\n" - "Tool for .fpl logfiles\n" - "\n" - "Usage:\n" - "\n" - " parsertool [flags] [ ...]\n" + "Tool for .fpl files (recordings)\n" "\n" "Usage:\n" "\n" - " fpltool [] [...]\n" + " fpltool [] [...]\n" "\n" "Where (availability of flags depends on , see below):\n" "\n", stdout); std::fputs(COMMON_FLAGS_HELP, stdout); std::fputs( - " -p / -P -- Show / don't show progress (default: automatic)\n" - " -f -- Force overwrite output (default: refuse to overwrite existing output files)\n" - " -c -- Compress output (e.g. ROS bags), -c -c to compress more\n" - " -x -- Add extra output, multiple -x can be given\n" - " -o -- Output to \n" - " -S -- Skip seconds from start of log (approximate!) (default: 0, i.e. no skip)\n" - " -D -- Process seconds of log (approximate!) (default: everything)\n" + "\n" + " -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" + " -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" + " -D, --duration -- Process seconds of log (default: everything)\n" + " -e, --formats -- Comma-separated list of output formats for the extract (default: all)\n" + " -- The command, see below\n" + " -- The .fpl (or .fpl.gz) file to process\n" " \n" - "Print information about the data in a .fpl logfile, along with status and meta data\n" + "The available s are:\n" + "\n" + " meta -- Print the meta data\n" + "\n" + " fpltool [-vqpP] meta \n" + "\n" + " The meta printed to stdout is suitable for further processing (yq, Python, ...).\n" + "\n" + " dump -- Print information about the data in a .fpl file\n" + "\n" + " fpltool [-vqpPx] dump \n" + "\n" + " By default it prints the statistics and meta data to stdout. With -x it prints a line for all data\n" + " in the (sequence number, offset into , size of chunk, type of data, debug info).\n" + " Adding another -x adds a hexdump.\n" + " Note that with -x this can print a lot of output to stdout!\n" + "\n" + " trim -- Trim a .fpl file to a shorter .fpl file\n" "\n" - " fpltool [-vqpPx] dump \n" + " fpltool [-vqpPfoc] -S -D \n" "\n" - "Print the meta data from a .fpl logfile (as YAML)\n" + " This shortens the by removing (skipping) seconds of data from the beginning\n" + " and using sections from that point in the . Note that this process is inaccurate\n" + " and the effective start time and duration of the resulting file may be off by 30 to 60 seconds.\n" + " Therefore, both the and must be at least 60 seconds.\n" "\n" - " fpltool [-vqpP] meta \n" -#ifdef FPSDK_USE_ROS1 + " rosbag -- Extract (some of) the data to a ROS bag\n" "\n" - "Generate a ROS bag from a .fpl logfile\n" + " This is an alias of 'extract -e ros'. See that for details.\n" "\n" - " fpltool [-vqpPfoSD] robag \n" -#endif + " extract -- Extract the data in a .fpl file\n" "\n" - "Trim start and/or end of .fpl logfile, i.e. extract a portion of the file. Note that this process is\n" - "inaccurate and the effective start time and duration of the resulting file may be off by 30 to 60 seconds.\n" - "Therefore, both the start time (-S) and the duration (-D) must be at least 60 seconds.\n" + " fpltool [-vqpPfocSD] [-e ] extract \n" "\n" - " fpltool [-vqpPfo] -S -D \n" + " The data is extracted to different files in the current directory. The files are named like the\n" + " with added suffixes and different file extension, depending on the kind of data that\n" + " is extracted. By default the data is extracted to all supported formats. Specify a comma-separated\n" + " list of formats in to limit to some formats. The available output data formats are:\n" "\n" - "Extract (som) non-ROS data from the .fpl logfile\n" + " jsonl -- All data in JSONL format (see below)\n" + " raw -- Stream messages (I/O messages, raw messages from GNSS receiver, ...)\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 format is used,\n" + " unless compression is used, in which case the mcap format is used.\n" "\n" - " fpltool [-vqpP] extract \n" + " The jsonl format consists of one JSON object per line. Depending on the data different fields are\n" + " available. The fields starting with a '_' (_type, ...) should always be present. The fields not\n" + " starting with a '_' are decoded values. What values can be decoded depends on the data in question,\n" + " the sensor configuration and software version, and the fpltool support for the data. Typically,\n" + " the following kind of JSON objects can be found in the exacted .jsonl file:\n" + " \n" + " Log meta data: { \"_type\": \"LOGMETA\", \"_yaml\": \"...\", ... }\n" + "\n" + " Where _yaml contains the raw meta data in YAML format. The additional fields (...) are decoded\n" + " from that (hw_uid, product_model, sw_version, etc.)\n" + "\n" + " Log status data: { \"_type\": \"LOGSTATUS\", \"_yaml\": \"...\", ... }\n" + "\n" + " Where _yaml contains the raw status data in YAML format. The additional fields (...) are decoded\n" + " from that (state, log_duration, log_size, etc.)\n" + "\n" + " ROS data: { \"_type\": \"ROSMSGBIN\", \"_topic\": \"...\", \"_msg\": \"...\", \"_stamp\": ..., ... }\n" + " Where _topic is the ROS topic (e.g. /imu/data), _msg is the message type (e.g. sensor_msgs/Imu),\n" + " _stamp is the recording (!) timestamp, and ... are the decoded data (e.g. header with frame_id,\n" + " seq and stamp, linear_acceleration, etc.). See the ROS1 documentation for the definitions of\n" + " the decoded fields.\n" + "\n" + " Stream messages: { \"_type\": \"STREAMMSG\", \"_stream\": \"...\", \"_stamp\": ..., \"_raw\": \"...\",\n" + " \"_raw_b64\": \"...\", \"_proto\": \"...\", \"_name\": \"...\", \"_seq\": ...,\n" + " \"_valid\": ..., ... } \n" + "\n" + " Where _stream is the stream name (userio, gnss1, ...), _stamp is the recording (!) timestamp,\n" + " _raw or _raw_b64 is the raw message data (_raw if data is non-binary, _raw_b64 = base64 encoded\n" + " binary data), _proto is the protocol name (FP_A, NMEA, ...), _name is the message name\n" + " (FP_A-ODOMETRY, NMEA-GN-RMC, ...), _seq is the sequence counter, and _valid indicates if the\n" + " message could be decoded (true) or not (false). Note that decoded fields (...) may be null.\n" + " See the protocol (UBX, FP_A, NMEA) specification documentation for the definitions of the\n" + " decoded fields.\n" + "\n" + " Files: { \"_type\": \"FILEDUMP\", \"_filename\": \"...\", \"_mtime\": \"...\", \"_stamp\": ...,\n" + " \"_data\": \"...\", \"_data_b64\": \"...\" } \n" + "\n" + " Where _filename is the filename of the file, _mtime its last modification time and _data or\n" + " or _data_b64 is the file contents (_data if it is non-binary, _data_b64 = base64 encoded data).\n" "\n" "Examples:\n" -#ifdef FPSDK_USE_ROS1 "\n" - " Create a some.bag from a some.fpl logfile:\n" + " Print the sensor UID of the sensor that recorded the fpl:\n" + "\n" + " fpltool -qq meta some.fpl | yq .hw_uid\n" + "\n" + " Save detailed info about recorded data to a text file:\n" + "\n" + " fpltool -x dump some.fpl > some.txt\n" "\n" - " fpltool rosbag some.fpl\n" + " Extract all data from a .fpl file:\n" "\n" - " Create a compressed another.bag with 2 minutes of data starting 60 seconds into some.fpl logfile:\n" + " fpltool extract some.fpl" + "\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 robag some.fpl # shortcut\n" + "\n" + " Create a compressed another.bag (res. another_bag) with 2 minutes of data starting 60 seconds into some.fpl:\n" "\n" " fpltool rosbag some.fpl -c -c -o another.bag -S 60 -D 120\n" -#endif "\n" - " Show meta data of some.fpl:\n" + " Check what is in the extracted ROS bag:\n" + "\n" + " rosbag info some.bag # ROS 1\n" + " ros2 bag info some_bag # ROS 2 (default, see above)\n" + " mcap inof some_bag/some.mcap # ROS 2 (with compression, see above)\n" + "\n" + " Check what is in some_userio.raw obtained by 'extract some.fpl':\n" + "\n" + " parsertool some_userio.raw > some_userio.txt\n" + "\n" + " Pretty-print some_all.jsonl obtained by 'extract some.fpl':\n" + "\n" + " jq < some_all.jsonl\n" + "\n" + " Pretty-print all some_all.jsonl obtained by 'extract some.fpl':\n" + "\n" + " jq < some_all.jsonl\n" "\n" " fpltool meta some.fpl\n" "\n" @@ -146,11 +250,19 @@ class FplToolOptions : public fpsdk::common::app::ProgramOptions "\n" " fpltool extract some.fpl\n" "\n" - " This results in various data files suitable for further processing. For example, the input/output\n" - " messages received/sent by the sensor can be processed by the parsertool:\n" + " Pretty-print all log status from some_all.jsonl obtained by 'extract some.fpl':\n" + "\n" + " jq '.|select(._type==\"LOGSTATUS\")' < some_all.jsonl\n" "\n" - " parsertool some_userio.raw\n" + " Pretty-print all IMU samples from some_all.jsonl obtained by 'extract some.fpl':\n" "\n" + " jq '.|select(._type==\"ROSMSGBIN\" and ._topic==\"/imu/data\")' < some_all.jsonl\n" + "\n" + " Plot IMU acceleration from some_all.jsonl obtained by 'extract some.fpl':\n" + "\n" + " jq -r '.|select(._topic==\"/imu/data\") | .linear_acceleration | join(\" \")' \\\n" + " < some_all.jsonl > plot.dat\n" + " gnuplot -p -e 'plot \"plot.dat\" using 1 w l t \"x\", \"\" using 2 w l t \"y\", \"\" using 3 w l t \"z\"'\n" "\n", stdout); // clang-format on } @@ -180,15 +292,19 @@ class FplToolOptions : public fpsdk::common::app::ProgramOptions compress_++; break; case 'S': - if (!fpsdk::common::string::StrToValue(argument, skip_)) { + if (!common::string::StrToValue(argument, skip_)) { ok = false; } break; case 'D': - if (!fpsdk::common::string::StrToValue(argument, duration_) || (duration_ < 1)) { + if (!common::string::StrToValue(argument, duration_) || (duration_ < 1)) { ok = false; } break; + case 'e': { + formats_ = common::string::StrSplit(argument, ","); + break; + } default: ok = false; break; @@ -236,16 +352,33 @@ class FplToolOptions : public fpsdk::common::app::ProgramOptions DEBUG("inputs_[%" PRIuMAX "] = '%s'", ix, inputs_[ix].c_str()); } DEBUG("output = '%s'", output_.c_str()); - DEBUG("overwrite = %s", fpsdk::common::string::ToStr(overwrite_)); + DEBUG("overwrite = %s", common::string::ToStr(overwrite_)); DEBUG("extra = %d", extra_); DEBUG("progress = %d", progress_); DEBUG("compress = %d", compress_); DEBUG("skip = %d", skip_); DEBUG("duration = %d", duration_); + DEBUG("formats = %s", common::string::StrJoin(formats_, " ").c_str()); return ok; } + std::string GetOutputPrefix(const std::string& input) const + { + if (!output_.empty()) { + return output_; + } + + std::string output = input; + common::string::StrReplace(output, ".gz", ""); + common::string::StrReplace(output, ".fpl", ""); + if ((skip_ > 0) || (duration_ > 0)) { + output += "_S" + std::to_string(skip_) + "_D" + std::to_string(duration_); + } + + return output; + } + private: bool progress_set_ = false; }; diff --git a/fpsdk_apps/fpltool/fpltool_rosbag.cpp b/fpsdk_apps/fpltool/fpltool_rosbag.cpp deleted file mode 100644 index 13f680e3..00000000 --- a/fpsdk_apps/fpltool/fpltool_rosbag.cpp +++ /dev/null @@ -1,215 +0,0 @@ -/** - * \verbatim - * ___ ___ - * \ \ / / - * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors - * / /\ \ License: see the LICENSE file - * /__/ \__\ - * \endverbatim - * - * @file - * @brief Fixposition SDK: fpltool rosbag - */ - -/* LIBC/STL */ -#include - -/* EXTERNAL */ - -/* Fixposition SDK */ -#include -#include -#include -#include -#include -#ifdef FPSDK_USE_ROS1 -# include -# include -#endif - -/* PACKAGE */ -#include "fpltool_rosbag.hpp" - -namespace fpsdk { -namespace apps { -namespace fpltool { -/* ****************************************************************************************************************** */ -#ifdef FPSDK_USE_ROS1 - -using namespace fpsdk::common::app; -using namespace fpsdk::common::fpl; -using namespace fpsdk::common::string; -using namespace fpsdk::common::path; -using namespace fpsdk::ros1::bagwriter; - -static std::string TopicName(const std::string& in_topic) -{ - if (in_topic == "/fusion_optim/imu_biases") { - return "/imu/biases"; - } else { - return in_topic; - } -} - -bool DoRosbag(const FplToolOptions& opts) -{ - if (opts.inputs_.size() != 1) { - WARNING("Need exactly one input file"); - return false; - } - const std::string input_fpl = opts.inputs_[0]; - - // Determine output file name - std::string output_bag; - if (opts.output_.empty()) { - output_bag = input_fpl; - StrReplace(output_bag, ".fpl", ""); - if ((opts.skip_ > 0) || (opts.duration_ > 0)) { - output_bag += "_S" + std::to_string(opts.skip_) + "-D" + std::to_string(opts.duration_); - } - output_bag += ".bag"; - } else { - output_bag = opts.output_; - } - - // Open input log - FplFileReader reader; - if (!reader.Open(input_fpl)) { - return false; - } - - // Open output bag - if (!opts.overwrite_ && PathExists(output_bag)) { - WARNING("Output file %s already exists", output_bag.c_str()); - return false; - } - BagWriter bag; - if (!bag.Open(output_bag, opts.compress_)) { - return false; - } - - // Prepare message for stream messages - fpsdk_ros1::ParserMsg stream_msg_ros; - stream_msg_ros.protocol = fpsdk::ros1::msgs::ParserMsg::PROTOCOL_UNSPECIFIED; - stream_msg_ros.seq = 0; - - // Handle SIGINT (C-c) to abort nicely - SigIntHelper sig_int; - - // Process log - NOTICE("Extracting %s to %s", input_fpl.c_str(), output_bag.c_str()); - FplMessage log_msg; - double progress = 0.0; - double rate = 0.0; - bool ok = true; - uint32_t time_into_log = 0; - while (!sig_int.ShouldAbort() && reader.Next(log_msg)) { - // Report progress - if (opts.progress_ > 0) { - if (reader.GetProgress(progress, rate)) { - INFO("Extracting... %.1f%% (%.0f MiB/s)\r", progress, rate); - } - } - - // Check if we want to skip this message - const bool skip = ((opts.skip_ > 0) && (time_into_log < opts.skip_)); - - // Maybe we can abort early - if ((opts.duration_ > 0) && (time_into_log > (opts.skip_ + opts.duration_))) { - DEBUG("abort early"); - break; - } - - // Process message - const auto log_type = log_msg.PayloadType(); - switch (log_type) { - case FplType::LOGMETA: { - break; - } - case FplType::ROSMSGDEF: { - const RosMsgDef rosmsgdef(log_msg); - if (rosmsgdef.valid_) { - bag.AddMsgDef( - TopicName(rosmsgdef.topic_name_), rosmsgdef.msg_name_, rosmsgdef.msg_md5_, rosmsgdef.msg_def_); - } else { - WARNING("Invalid ROSMSGDEF"); - ok = false; - } - break; - } - case FplType::ROSMSGBIN: - if (!skip) { - const RosMsgBin rosmsgbin(log_msg); - if (!rosmsgbin.valid_ || - !bag.WriteMessage(rosmsgbin.msg_data_, TopicName(rosmsgbin.topic_name_), rosmsgbin.rec_time_)) { - WARNING("Invalid ROSMSGBIN"); - ok = false; - } - } - break; - case FplType::STREAMMSG: - if (!skip) { - const StreamMsg streammsg(log_msg); - if (streammsg.valid_) { - stream_msg_ros.stamp = { streammsg.rec_time_.sec_, streammsg.rec_time_.nsec_ }; - stream_msg_ros.data = streammsg.msg_data_; - // stream_msg_ros.name = ...; // @todo run data through parser - // stream_msg_ros.protocol = ...; // @todo run data through parser - const std::string topic = - "/" + (streammsg.stream_name_ == "corr" ? "ntrip" : streammsg.stream_name_) + "/raw"; - bag.WriteMessage(stream_msg_ros, topic, streammsg.rec_time_); - // stream_msg_ros.seq++; // @todo per topic - } else { - WARNING("Invalid STREAMMSG"); - ok = false; - } - } - break; - case FplType::LOGSTATUS: { - const LogStatus logstatus(log_msg); - if (logstatus.valid_) { - time_into_log = logstatus.log_duration_; - } - break; - } - case FplType::FILEDUMP: - case FplType::BLOB: - case FplType::UNSPECIFIED: - case FplType::INT_D: - case FplType::INT_F: - case FplType::INT_X: - break; - } - } - - // We were interrupted - if (sig_int.ShouldAbort()) { - ok = false; - } - - bag.Close(); - - const double bag_size = FileSize(output_bag) / 1024.0 / 1024.0; - if (ok) { - INFO("Wrote bag %s (%.0f MiB)", output_bag.c_str(), bag_size); - } else { - WARNING("Incomplete bag %s (%.0f MiB)", output_bag.c_str(), bag_size); - } - - return ok; -} - -/* ****************************************************************************************************************** */ -#else // FPSDK_USE_ROS1 - -bool DoRosbag(const FplToolOptions& opts) -{ - WARNING("Command %s unavailable, ROS functionality is not compiled in", opts.command_str_.c_str()); - return false; -} -#endif // FPSDK_USE_ROS1 - -/* ****************************************************************************************************************** */ -} // namespace fpltool -} // namespace apps -} // namespace fpsdk diff --git a/fpsdk_apps/fpltool/fpltool_trim.cpp b/fpsdk_apps/fpltool/fpltool_trim.cpp index ecfc81e1..4a4c877b 100644 --- a/fpsdk_apps/fpltool/fpltool_trim.cpp +++ b/fpsdk_apps/fpltool/fpltool_trim.cpp @@ -53,14 +53,7 @@ bool DoTrim(const FplToolOptions& opts) } // Determine output file name - std::string output_fpl; - if (opts.output_.empty()) { - output_fpl = input_fpl; - StrReplace(output_fpl, ".fpl", ""); - output_fpl += "_S" + std::to_string(opts.skip_) + "-D" + std::to_string(opts.duration_) + ".fpl"; - } else { - output_fpl = opts.output_; - } + std::string output_fpl = opts.GetOutputPrefix(input_fpl) + ".fpl" + (opts.compress_ ? ".gz" : ""); // Open input log FplFileReader reader; @@ -99,8 +92,8 @@ bool DoTrim(const FplToolOptions& opts) // Report progress if (opts.progress_ > 0) { if (reader.GetProgress(progress, rate)) { - INFO("Processing... %.1f%% (%.0f MiB/s) -- %" PRIu64 " msgs\r", progress, rate, n_used); - n_used = 0; + INFO("Processing... %.1f%% (%.0f MiB/s) -- time into log %" PRIu32 ", used %" PRIu64 " msgs\r", + progress, rate, time_into_log, n_used); } } diff --git a/fpsdk_apps/fpltool/fpltool_utils.cpp b/fpsdk_apps/fpltool/fpltool_utils.cpp new file mode 100644 index 00000000..6fae7266 --- /dev/null +++ b/fpsdk_apps/fpltool/fpltool_utils.cpp @@ -0,0 +1,300 @@ +/** + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ + * \endverbatim + * + * @file + * @brief Fixposition SDK: fpltool extract + */ + +/* LIBC/STL */ +#include + +/* EXTERNAL */ + +/* Fixposition SDK */ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#if defined(FPSDK_USE_ROS1) +# include +#endif + +/* PACKAGE */ +#include "fpltool_utils.hpp" + +namespace fpsdk { +namespace apps { +namespace fpltool { +/* ****************************************************************************************************************** */ + +using namespace fpsdk::common::app; +using namespace fpsdk::common::fpl; +using namespace fpsdk::common::parser; +using namespace fpsdk::common::parser::fpa; +using namespace fpsdk::common::parser::fpb; +using namespace fpsdk::common::parser::nmea; +using namespace fpsdk::common::path; +using namespace fpsdk::common::ros1; +using namespace fpsdk::common::string; +using namespace fpsdk::common::types; + +// --------------------------------------------------------------------------------------------------------------------- + +OutputFileHelper::OutputFileHelper(const std::string& prefix, const FplToolOptions& opts) /* clang-format off */ : + prefix_ { prefix }, + opts_ { opts } // clang-format on +{ +} +OutputFileHelper::~OutputFileHelper() +{ + CloseAll(); +} + +bool OutputFileHelper::WriteData(const std::string& name, const std::vector& data) +{ + auto f = GetOutputFile(name); + return f ? f->Write(data) : false; +} + +bool OutputFileHelper::WriteJson(const std::string& name, const nlohmann::json& json) +{ + auto f = GetOutputFile(name); + if (!f) { + return false; + } + return f->Write(json.dump()) && f->Write("\n"); +} + +bool OutputFileHelper::WriteStreamMsg(const std::string& name, const StreamMsg& streammsg, const ParserMsg& parsermsg) +{ + nlohmann::json jdata = streammsg; // magic to_json() (_type, _stamp, _stream, _raw/_raw_b64) + jdata.update(parsermsg); // magic to_json() (_proto, _name, _seq, _info, _raw/_raw_b64) + + // We can decode some messages of some protocols + if (msg_.proto_ == Protocol::FP_A) { + jdata.update(FpaDecodeMessage(msg_.data_)); // magic to_json() + } else if (msg_.proto_ == Protocol::NMEA) { + jdata.update(NmeaDecodeMessage(msg_.data_)); // magic to_json() + } else if (msg_.proto_ == Protocol::FP_B) { + jdata.update(to_json_FP_B(msg_)); // magic to_json() + } + return WriteJson(name, jdata); +} + +void OutputFileHelper::CloseAll(const bool ok) +{ + for (auto& file : files_) { + const std::string path = file.second->Path(); + file.second->Close(); + const auto size_str = OutputSizeStr(path); + if (ok) { + INFO("Wrote file %s (%s)", path.c_str(), size_str.c_str()); + } else { + WARNING("Incomplete file %s (%s)", path.c_str(), size_str.c_str()); + } + } + files_.clear(); +} + +OutputFile* OutputFileHelper::GetOutputFile(const std::string& name) +{ + auto file = files_.find(name); + if (file == files_.end()) { + const std::string path = prefix_ + "_" + name; + file = files_.emplace(name, std::make_unique()).first; + if (!opts_.overwrite_ && PathExists(path)) { + WARNING("Output file %s already exists", path.c_str()); + return nullptr; + } + NOTICE("Extracting to %s", path.c_str()); + if (!file->second->Open(path)) { + return nullptr; + } + } + return file->second.get(); +} + +// --------------------------------------------------------------------------------------------------------------------- + +ParserMsgHelper::ParserMsgHelper() +{ +} +ParserMsgHelper::~ParserMsgHelper() +{ +} + +void ParserMsgHelper::UpdateParserMsg(const common::fpl::StreamMsg& streammsg) +{ + parser_.Reset(); + if (!parser_.Add(streammsg.msg_data_) || !parser_.Process(msg_) || + (msg_.data_.size() != streammsg.msg_data_.size())) { + msg_.proto_ = Protocol::OTHER; + msg_.name_ = ProtocolStr(Protocol::OTHER); + msg_.data_ = streammsg.msg_data_; + } + msg_.info_.clear(); + auto seq = seq_.find(streammsg.stream_name_); + if (seq == seq_.end()) { + seq = seq_.emplace(streammsg.stream_name_, 1).first; + } + msg_.seq_ = seq->second++; + +#if defined(FPSDK_USE_ROS1) + rosmsg_.stamp = { streammsg.rec_time_.sec_, streammsg.rec_time_.nsec_ }; + rosmsg_.name = msg_.name_; + rosmsg_.seq++; + rosmsg_.data = msg_.data_; + switch (msg_.proto_) { // clang-format off + case Protocol::FP_A: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_FP_A; break; + case Protocol::FP_B: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_FP_B; break; + case Protocol::NMEA: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_NMEA; break; + case Protocol::UBX: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_UBX; break; + case Protocol::RTCM3: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_RTCM3; break; + case Protocol::UNI_B: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_UNI_B; break; + case Protocol::NOV_B: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_NOV_B; break; + case Protocol::SPARTN: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_SPARTN; break; + case Protocol::OTHER: rosmsg_.protocol = fpsdk_ros1::ParserMsg::PROTOCOL_OTHER; break; + } // clang-format on +#elif defined(FPSDK_USE_ROS2) + // @todo implement for ROS2 +#endif +} + +const ParserMsg& ParserMsgHelper::GetParserMsg(const bool make_info) const +{ + if (make_info) { + msg_.MakeInfo(); + } + return msg_; +} + +#if defined(FPSDK_USE_ROS1) +const fpsdk_ros1::ParserMsg& ParserMsgHelper::GetRosMsg() +{ + msg_.MakeInfo(); + rosmsg_.info = msg_.info_; + return rosmsg_; +} +#elif defined(FPSDK_USE_ROS2) +// @todo implement for ROS2 +#endif + +// --------------------------------------------------------------------------------------------------------------------- + +RosMsgHelper::RosMsgHelper() +{ +} + +RosMsgHelper::~RosMsgHelper() +{ +} + +void RosMsgHelper::AddDef(const RosMsgDef& rosmsgdef) +{ + if (rosmsgdef.valid_ && (defs_.find(rosmsgdef.topic_name_) == defs_.end())) { + DEBUG("RosMsgHelper: %s", rosmsgdef.info_.c_str()); + defs_.emplace(rosmsgdef.topic_name_, rosmsgdef); + } +} + +bool RosMsgHelper::ToJson(const RosMsgBin& rosmsgbin, nlohmann::json& jdata) +{ + if (!rosmsgbin.valid_) { + return false; + } + + const auto& entry = defs_.find(rosmsgbin.topic_name_); + if (entry == defs_.end()) { + WARNING_THR(1000, "Missing ROSMSGDEF for ROSMSGBIN %s", rosmsgbin.info_.c_str()); + return false; + } + + const auto& rosmsgdef = entry->second; + TRACE("ROSMSGBIN %s using ROSMSGDEF %s", rosmsgbin.info_.c_str(), rosmsgdef.info_.c_str()); + + // Try the implemented conversions + bool ok = false; + try { + if (RosMsgToJson(rosmsgdef, rosmsgbin, jdata) || + RosMsgToJson(rosmsgdef, rosmsgbin, jdata) || + RosMsgToJson(rosmsgdef, rosmsgbin, jdata) || + RosMsgToJson(rosmsgdef, rosmsgbin, jdata) || + RosMsgToJson(rosmsgdef, rosmsgbin, jdata)) { + jdata["_type"] = FplTypeStr(FplType::ROSMSGBIN); + jdata["_msg"] = rosmsgdef.msg_name_; + jdata["_topic"] = rosmsgbin.topic_name_; + jdata["_stamp"] = rosmsgbin.rec_time_; + ok = true; + } else { + throw std::runtime_error("conversion not implemented"); + } + } catch (std::exception& ex) { + WARNING( + "ROSMSGBIN %s ROSMSGDEF %s ToJson fail: %s", rosmsgbin.info_.c_str(), rosmsgdef.info_.c_str(), ex.what()); + } + return ok; +} + +// --------------------------------------------------------------------------------------------------------------------- + +std::string FileDumpOutName(const common::fpl::FileDump& filedump) +{ + const auto parts = StrSplit(filedump.filename_, ".", 2); + std::string outname = parts[0]; + const auto m = filedump.mtime_.GetUtcTime(0); + outname += Sprintf("_%04d%02d%02d-%02d%02d%02.0f", m.year_, m.month_, m.day_, m.hour_, m.min_, m.sec_); + if (parts.size() > 1) { + outname += "." + parts[1]; + } + return outname; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const std::string& FixTopicName(const std::string& in_topic) +{ + if (in_topic == "/fusion_optim/imu_biases") { + static std::string str = "/imu/biases"; + return str; + } else { + return in_topic; + } +} + +// --------------------------------------------------------------------------------------------------------------------- + +std::string OutputSizeStr(const std::string& path) +{ + const double size = (double)(PathIsDirectory(path) ? DirSize(path) : FileSize(path)); + if (size < 1024.0) { + return Sprintf("%.0f B", size); + } else if (size < (1024.0 * 1024.0)) { + return Sprintf("%.1f KiB", size / 1024.0); + } else { + return Sprintf("%.1f MiB", size / 1024.0 / 1024.0); + } +} + +/* ****************************************************************************************************************** */ +} // namespace fpltool +} // namespace apps +} // namespace fpsdk diff --git a/fpsdk_apps/fpltool/fpltool_utils.hpp b/fpsdk_apps/fpltool/fpltool_utils.hpp new file mode 100644 index 00000000..af7d4925 --- /dev/null +++ b/fpsdk_apps/fpltool/fpltool_utils.hpp @@ -0,0 +1,104 @@ +/** + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ + * \endverbatim + * + * @file + * @brief Fixposition SDK: fpltool utils + */ +#ifndef __FPSDK_APPS_FPLTOOL_FPLTOOL_UTILS_HPP__ +#define __FPSDK_APPS_FPLTOOL_FPLTOOL_UTILS_HPP__ + +/* LIBC/STL */ +#include +#include +#include + +/* EXTERNAL */ +#include + +/* Fixposition SDK */ +#include +#include +#include +#include +#if defined(FPSDK_USE_ROS1) +# include +#endif + +/* PACKAGE */ +#include "fpltool_opts.hpp" + +namespace fpsdk { +namespace apps { +namespace fpltool { +/* ****************************************************************************************************************** */ + +class OutputFileHelper +{ + public: + OutputFileHelper(const std::string& prefix, const FplToolOptions& opts); + ~OutputFileHelper(); + + bool WriteData(const std::string& name, const std::vector& data); + bool WriteJson(const std::string& name, const nlohmann::json& json); + bool WriteStreamMsg( + const std::string& name, const common::fpl::StreamMsg& streammsg, const common::parser::ParserMsg& parsermsg); + void CloseAll(const bool ok = false); + + private: + std::string prefix_; + FplToolOptions opts_; + std::map> files_; + common::parser::Parser parser_; + common::parser::ParserMsg msg_; + common::path::OutputFile* GetOutputFile(const std::string& name); +}; + +class ParserMsgHelper +{ + public: + ParserMsgHelper(); + ~ParserMsgHelper(); + void UpdateParserMsg(const common::fpl::StreamMsg& streammsg); + const common::parser::ParserMsg& GetParserMsg(const bool make_info = false) const; +#if defined(FPSDK_USE_ROS1) // || defined(FPSDK_USE_ROS2) // @todo implement for ROS2 + const fpsdk_ros1::ParserMsg& GetRosMsg(); +#endif + + private: + common::parser::Parser parser_; + common::parser::ParserMsg msg_; + std::map seq_; +#if defined(FPSDK_USE_ROS1) // || defined(FPSDK_USE_ROS2) // @todo implement for ROS2 + fpsdk_ros1::ParserMsg rosmsg_; +#endif +}; + +class RosMsgHelper +{ + public: + RosMsgHelper(); + ~RosMsgHelper(); + void AddDef(const common::fpl::RosMsgDef& rosmsgdef); + bool ToJson(const common::fpl::RosMsgBin& rosmsgbin, nlohmann::json& jdata); + + private: + std::map defs_; +}; + +std::string FileDumpOutName(const common::fpl::FileDump& filedump); + +const std::string& FixTopicName(const std::string& in_topic); + +std::string OutputSizeStr(const std::string& path); + +/* ****************************************************************************************************************** */ +} // namespace fpltool +} // namespace apps +} // namespace fpsdk +#endif // __FPSDK_APPS_FPLTOOL_FPLTOOL_UTILS_HPP__ diff --git a/fpsdk_apps/package.xml b/fpsdk_apps/package.xml index c7f6755c..ae469c25 100644 --- a/fpsdk_apps/package.xml +++ b/fpsdk_apps/package.xml @@ -9,5 +9,8 @@ cmake fpsdk_common + fpsdk_ros1 + fpsdk_ros2 diff --git a/fpsdk_apps/parsertool/parsertool.cpp b/fpsdk_apps/parsertool/parsertool.cpp index c2a72890..214800fb 100644 --- a/fpsdk_apps/parsertool/parsertool.cpp +++ b/fpsdk_apps/parsertool/parsertool.cpp @@ -77,6 +77,7 @@ class ParserToolOptions : public ProgramOptions "\n", stdout); std::fputs(COMMON_FLAGS_HELP, stdout); std::fputs( + "\n" " -x, --hexdump -- Print hexdump of each message, not with -c\n" " -s, --save -- Save each (!) message into a separate (!) file in the current directory, not with -c\n" " -f , --filter \n" diff --git a/fpsdk_apps/timeconv/timeconv.cpp b/fpsdk_apps/timeconv/timeconv.cpp index 0e8529a6..270be36f 100644 --- a/fpsdk_apps/timeconv/timeconv.cpp +++ b/fpsdk_apps/timeconv/timeconv.cpp @@ -62,14 +62,16 @@ class TimeConvOptions : public ProgramOptions "\n" "Usage:\n" "\n" - " timeconv [flags] []\n" + " timeconv [flags] []\n" "\n" "Where:\n" "\n", stdout); std::fputs(COMMON_FLAGS_HELP, stdout); std::fputs( + "\n" " -i, --ignore -- Ignore errors in input (only from stdin, not for input on command line)\n" " -p , --prec -- Precision (number of fractional digits) for float outputs (default: 3)\n" + " -- Input data, see below\n" "\n" "A single is either provided on the command line or on stdin, one per line.\n" "The program converts the input to the desired output. One has the following format:\n" diff --git a/fpsdk_apps/yaml2shell/yaml2shell.cpp b/fpsdk_apps/yaml2shell/yaml2shell.cpp index 0041e1c7..6cd57ae3 100644 --- a/fpsdk_apps/yaml2shell/yaml2shell.cpp +++ b/fpsdk_apps/yaml2shell/yaml2shell.cpp @@ -73,12 +73,13 @@ class YamlToShellOptions : public ProgramOptions "\n" "Usage:\n" "\n" - " yaml2shell [flags] [\n" + " yaml2shell [flags] []\n" "\n" "Where:\n" "\n", stdout); std::fputs(COMMON_FLAGS_HELP, stdout); std::fputs( + "\n" " -p -- Prefix for variable names (default: none)\n" " -o -- Output to file instead of stdout\n" " -u -- Uppercase variable names\n" diff --git a/fpsdk_common/CMakeLists.txt b/fpsdk_common/CMakeLists.txt index db892c1e..33502502 100644 --- a/fpsdk_common/CMakeLists.txt +++ b/fpsdk_common/CMakeLists.txt @@ -1,7 +1,9 @@ # GENERAL ============================================================================================================== +message(STATUS "fpsdk: ----- ${CMAKE_CURRENT_SOURCE_DIR} -----") cmake_minimum_required(VERSION 3.16) include(cmake/setup.cmake) +include(cmake/ros.cmake) project(fpsdk_common LANGUAGES CXX @@ -25,7 +27,7 @@ endif() if(NOT CMAKE_BUILD_TYPE STREQUAL "Debug") add_compile_definitions(NDEBUG) endif() -if (NOT CMAKE_BUILD_TYPE STREQUAL "Release") +if(NOT CMAKE_BUILD_TYPE STREQUAL "Release") add_compile_definitions(BOOST_STACKTRACE_USE_BACKTRACE) add_compile_definitions(BOOST_STACKTRACE_USE_ADDR2LINE) set(FPSDK_BOOST_STACKTRACE 1) @@ -35,14 +37,14 @@ endif() # DEPENDENCIES ========================================================================================================= if(FPSDK_BOOST_STACKTRACE) - if (FPSDK_LEGACY_BOOST_CMAKE) # e.g. Buildroot 2022.08 + if(FPSDK_LEGACY_BOOST_CMAKE) # e.g. Buildroot 2022.08 find_package(Boost REQUIRED COMPONENTS stacktrace_basic stacktrace_backtrace stacktrace_addr2line) else() find_package(Boost CONFIG REQUIRED COMPONENTS stacktrace_basic stacktrace_backtrace stacktrace_addr2line) endif() set(Boost_TARGETS "Boost::headers;Boost::stacktrace_basic;Boost::stacktrace_backtrace;Boost::stacktrace_addr2line") else() - if (FPSDK_LEGACY_BOOST_CMAKE) + if(FPSDK_LEGACY_BOOST_CMAKE) find_package(Boost REQUIRED) else() find_package(Boost CONFIG REQUIRED) @@ -58,14 +60,14 @@ find_package(PkgConfig REQUIRED) pkg_search_module(OpenSSL REQUIRED IMPORTED_TARGET openssl) # PROJ is optional, unless explicitly requested with -DFPSDK_USE_PROJ=ON -if (FPSDK_USE_PROJ STREQUAL "ON") +if(FPSDK_USE_PROJ STREQUAL "ON") find_package(PROJ 9.4 REQUIRED CONFIG) message(STATUS "fpsdk: Using PROJ (${PROJ_VERSION})") elseif(FPSDK_USE_PROJ STREQUAL "OFF") message(STATUS "fpsdk: Not using PROJ") else() find_package(PROJ 9.4 CONFIG QUIET) - if (${PROJ_FOUND}) + if(${PROJ_FOUND}) message(STATUS "fpsdk: Using PROJ (${PROJ_VERSION})") set(FP_FPSDK_USE_PROJ ON) else() @@ -74,16 +76,33 @@ else() endif() endif() +if(FPSDK_USE_ROS1) + 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) + endif() + fpsdk_cmake_local_ros1_package(std_msgs) + fpsdk_cmake_local_ros1_package(sensor_msgs) + fpsdk_cmake_local_ros1_package(geometry_msgs) + fpsdk_cmake_local_ros1_package(tf2_msgs) + fpsdk_cmake_local_ros1_package(nav_msgs) +endif() + fpsdk_save_versions(FILE ${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}_versions.txt PACKAGES CMAKE Boost yaml-cpp ZLIB Eigen3 nlohmann_json OpenSSL PROJ) - # SHARED LIBRARY ======================================================================================================= file(GLOB CPP_FILES src/*.cpp src/parser/*.cpp src/stream/*.cpp) add_library(${PROJECT_NAME} SHARED ${CPP_FILES}) -if (FPSDK_USE_PROJ) +if(FPSDK_USE_PROJ) target_compile_definitions(${PROJECT_NAME} PUBLIC FPSDK_USE_PROJ=1) else() target_compile_definitions(${PROJECT_NAME} PUBLIC FPSDK_USE_PROJ=0) @@ -95,6 +114,14 @@ target_include_directories(${PROJECT_NAME} $ ) +if(NOT FPSDK_USE_ROS1) + target_include_directories(${PROJECT_NAME} + PUBLIC + $ + $ + ) +endif() + set(FPSDK_COMMON_TRANS_DEPS "Eigen3;yaml-cpp") target_link_libraries(${PROJECT_NAME} @@ -102,6 +129,11 @@ target_link_libraries(${PROJECT_NAME} Eigen3::Eigen yaml-cpp PRIVATE + $ + $ + $ + $ + $ ${Boost_TARGETS} ${CMAKE_DL_LIBS} ZLIB::ZLIB @@ -130,6 +162,10 @@ install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${PROJECT_INCLUDE_DIR} ) +install(DIRECTORY rosnoros + DESTINATION ${PROJECT_INCLUDE_DIR} +) + # Library, tools install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}-targets @@ -205,6 +241,7 @@ 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 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/cmake/ros.cmake b/fpsdk_common/cmake/ros.cmake new file mode 100644 index 00000000..ea1424e8 --- /dev/null +++ b/fpsdk_common/cmake/ros.cmake @@ -0,0 +1,21 @@ + +# Find ROS1 package and create a target for it +macro(fpsdk_cmake_find_ros1_package ROS_PACKAGE) + if(NOT TARGET ros1::${ROS_PACKAGE}) + # Create a target for the ROS library + add_library(ros1_${ROS_PACKAGE} INTERFACE) + add_library(ros1::${ROS_PACKAGE} ALIAS ros1_${ROS_PACKAGE}) + find_package(${ROS_PACKAGE} REQUIRED) + target_include_directories(ros1_${ROS_PACKAGE} INTERFACE ${${ROS_PACKAGE}_INCLUDE_DIRS}) + target_link_libraries(ros1_${ROS_PACKAGE} INTERFACE ${${ROS_PACKAGE}_LIBRARIES}) + endif() +endmacro() + +# Use shipped ROS1 "packages" (only for fpsdk_common!) +macro(fpsdk_cmake_local_ros1_package ROS_PACKAGE) + if(NOT TARGET ros1::${ROS_PACKAGE}) + add_library(ros1_${ROS_PACKAGE} INTERFACE) + add_library(ros1::${ROS_PACKAGE} ALIAS ros1_${ROS_PACKAGE}) + target_include_directories(ros1_${ROS_PACKAGE} INTERFACE ${PROJECT_SOURCE_DIR}/rosnoros) + endif() +endmacro() diff --git a/fpsdk_common/cmake/testing.cmake b/fpsdk_common/cmake/testing.cmake index 4b2c7ebd..5f7c4f3d 100644 --- a/fpsdk_common/cmake/testing.cmake +++ b/fpsdk_common/cmake/testing.cmake @@ -55,14 +55,6 @@ if(BUILD_TESTING) ${ADD_GTEST_SOURCES} ) - target_include_directories(${GTEST_PREFIX}_${ADD_GTEST_TARGET} - PRIVATE - ${ADD_GTEST_INCLUDE_DIRS} - ) - target_link_directories(${GTEST_PREFIX}_${ADD_GTEST_TARGET} - PRIVATE - ${ADD_GTEST_LINK_DIRS} - ) target_link_libraries(${GTEST_PREFIX}_${ADD_GTEST_TARGET} PRIVATE GTest::gtest_main diff --git a/fpsdk_common/doc/doc.hpp b/fpsdk_common/doc/doc.hpp index 2573d38d..0e904e21 100644 --- a/fpsdk_common/doc/doc.hpp +++ b/fpsdk_common/doc/doc.hpp @@ -39,6 +39,7 @@ namespace common { - @subpage FPSDK_COMMON_MATH - @subpage FPSDK_COMMON_PARSER - @subpage FPSDK_COMMON_PATH + - @subpage FPSDK_COMMON_ROS1 - @subpage FPSDK_COMMON_STRING - @subpage FPSDK_COMMON_TIME - @subpage FPSDK_COMMON_THREAD diff --git a/fpsdk_common/include/fpsdk_common/app.hpp b/fpsdk_common/include/fpsdk_common/app.hpp index f0f5506c..66dab6bc 100644 --- a/fpsdk_common/include/fpsdk_common/app.hpp +++ b/fpsdk_common/include/fpsdk_common/app.hpp @@ -226,8 +226,8 @@ class ProgramOptions static constexpr const char* COMMON_FLAGS_HELP = /* clang-format off */ " -h, --help -- Print program help screen, and exit\n" " -V, --version -- Print program, version and license information, and exit\n" - " -v, --verbose / -q, --quiet\n" - " -- Increase / decrease logging verbosity, multiple flags accumulate\n" + " -v, --verbose -- Increase logging verbosity, multiple flags accumulate\n" + " -q, --quiet -- Decrease logging verbosity, multiple flags accumulate\n" " -J, --journal -- Use systemd journal logging markers instead of colours\n"; // clang-format on std::string app_name_; //!< App name diff --git a/fpsdk_common/include/fpsdk_common/fpl.hpp b/fpsdk_common/include/fpsdk_common/fpl.hpp index 775c3e73..04af8c0b 100644 --- a/fpsdk_common/include/fpsdk_common/fpl.hpp +++ b/fpsdk_common/include/fpsdk_common/fpl.hpp @@ -8,9 +8,9 @@ * \endverbatim * * @file - * @brief Fixposition SDK: .fpl logfile helpers + * @brief Fixposition SDK: .fpl utilities * - * @page FPSDK_COMMON_FPL .fpl logfile utilities + * @page FPSDK_COMMON_FPL .fpl utilities * * **API**: fpsdk_common/fpl.hpp and fpsdk::common::fpl * @@ -26,8 +26,6 @@ #include /* EXTERNAL */ -#include -#include /* PACKAGE */ #include "time.hpp" @@ -35,7 +33,7 @@ namespace fpsdk { namespace common { /** - * @brief .fpl logfile utilities + * @brief .fpl utilities */ namespace fpl { /* ****************************************************************************************************************** */ @@ -51,7 +49,7 @@ enum class FplType : uint16_t LOGMETA = 0x0105, //!< Logfile meta data STREAMMSG = 0x0106, //!< Stream message raw data with timestamp LOGSTATUS = 0x0107, //!< Logging status - FILEDUMP = 0x0108, //!< Dump of an entire (small, and text) file + FILEDUMP = 0x0108, //!< Dump of an entire (small) file BLOB = 0xbaad, //!< Arbitrary data, not FplMessage framing INT_D = 0xffaa, //!< Fixposition internal use only INT_F = 0xffbb, //!< Fixposition internal use only @@ -261,6 +259,8 @@ class FplFileReader */ struct LogMeta { + LogMeta() = default; //!< Default ctor + /** * @brief Constructor * @@ -287,6 +287,8 @@ struct LogMeta */ struct LogStatus { + LogStatus() = default; //!< Default ctor + /** * @brief Constructor * @@ -332,13 +334,15 @@ struct LogStatus */ struct RosMsgDef { + RosMsgDef() = default; //!< Default ctor + /** * @brief Constructor * * @param[in] log_msg .fpl log message */ RosMsgDef(const FplMessage& log_msg); - bool valid_; //!< Data valid, successfully extracted from message + bool valid_ = false; //!< Data valid, successfully extracted from message std::string info_; //!< Stringification of (some of the) data, for debugging std::string topic_name_; //!< The topic name std::string msg_name_; //!< The message name (a.k.a. data type) @@ -353,13 +357,15 @@ struct RosMsgDef */ struct RosMsgBin { + RosMsgBin() = default; //!< Default ctor + /** * @brief Constructor * * @param[in] log_msg .fpl log message */ RosMsgBin(const FplMessage& log_msg); - bool valid_; //!< Data valid, successfully extracted from message + bool valid_ = false; //!< Data valid, successfully extracted from message std::string info_; //!< Stringification of (some of the) data, for debugging std::string topic_name_; //!< The topic name time::RosTime rec_time_; //!< Recording timestamp @@ -373,13 +379,15 @@ struct RosMsgBin */ struct StreamMsg { + StreamMsg() = default; //!< Default ctor + /** * @brief Constructor * * @param[in] log_msg .fpl log message */ StreamMsg(const FplMessage& log_msg); - bool valid_; //!< Data valid, successfully extracted from message + bool valid_ = false; //!< Data valid, successfully extracted from message std::string info_; //!< Stringification of (some of the) data, for debugging time::RosTime rec_time_; //!< Recording timestamp std::string stream_name_; //!< Stream name diff --git a/fpsdk_common/include/fpsdk_common/logging.hpp b/fpsdk_common/include/fpsdk_common/logging.hpp index a2106c0f..6e564fb9 100644 --- a/fpsdk_common/include/fpsdk_common/logging.hpp +++ b/fpsdk_common/include/fpsdk_common/logging.hpp @@ -381,7 +381,7 @@ void LoggingHexdump(const LoggingLevel level, const uint8_t* data, const std::si static uint32_t __repeat = 0; \ const uint32_t __now = ::fpsdk::common::time::GetMillis(); \ __repeat++; \ - if ((__now - __last) >= (_millis_)) { \ + if ((__last == 0) || ((__now - __last) >= (_millis_))) { \ __last = __now; \ ::fpsdk::common::logging::LoggingPrint( \ ::fpsdk::common::logging::LoggingLevel::_level_, __repeat, __VA_ARGS__); \ diff --git a/fpsdk_common/include/fpsdk_common/parser/fpa.hpp b/fpsdk_common/include/fpsdk_common/parser/fpa.hpp index aba224c7..2e5032b4 100644 --- a/fpsdk_common/include/fpsdk_common/parser/fpa.hpp +++ b/fpsdk_common/include/fpsdk_common/parser/fpa.hpp @@ -166,6 +166,15 @@ enum class FpaInitStatus : int GLOBAL_INIT = '2', //!< Globally initialised }; // clang-format on +/** + * @brief Stringify initialisation status + * + * @param[in] status The initialisation status + * + * @returns the stringification of the initialisation status + */ +const char* FpaInitStatusStr(const FpaInitStatus status); + /** * @brief FP_A legacy fusion status */ @@ -179,6 +188,15 @@ enum class FpaFusionStatusLegacy : int VIO_GNSS = '4' //!< Visual-inertial-GNSS fusion }; // clang-format on +/** + * @brief Stringify legacy fusion status + * + * @param[in] status The legacy fusion status + * + * @returns the stringification of the legacy fusion status + */ +const char* FpaFusionStatusLegacyStr(const FpaFusionStatusLegacy status); + /** * @brief FP_A fusion measurement status */ @@ -190,6 +208,15 @@ enum class FpaMeasStatus : int DEGRADED = '2', //!< Degraded }; // clang-format on +/** + * @brief Stringify fusion measurement status + * + * @param[in] status The fusion measurement status + * + * @returns the stringification of the fusion measurement status + */ +const char* FpaMeasStatusStr(const FpaMeasStatus status); + /** * @brief FP_A IMU bias status */ @@ -203,7 +230,16 @@ enum class FpaImuStatus : int }; // clang-format on /** - * @brief FP_A Legacy IMU bias status + * @brief Stringify IMU bias status + * + * @param[in] status The IMU bias status + * + * @returns the stringification of the IMU bias status + */ +const char* FpaImuStatusStr(const FpaImuStatus status); + +/** + * @brief FP_A legacy IMU bias status */ enum class FpaImuStatusLegacy : int { // clang-format off @@ -212,6 +248,15 @@ enum class FpaImuStatusLegacy : int CONVERGED = '1', //!< Converged }; // clang-format on +/** + * @brief Stringify legacy IMU bias status + * + * @param[in] status The legacy IMU bias status + * + * @returns the stringification of the legacy IMU bias status + */ +const char* FpaImuStatusLegacyStr(const FpaImuStatusLegacy status); + /** * @brief FP_A IMU variance */ @@ -227,6 +272,15 @@ enum class FpaImuNoise : int RESERVED7 = '7', //!< Reserved }; // clang-format on +/** + * @brief Stringify IMU variance + * + * @param[in] noise The IMU variance + * + * @returns the stringification of the IMU variance + */ +const char* FpaImuNoiseStr(const FpaImuNoise noise); + /** * @brief FP_A IMU accelerometer and gyroscope convergence */ @@ -243,6 +297,15 @@ enum class FpaImuConv : int IDLE = '7', //!< Idle }; // clang-format on +/** + * @brief Stringify IMU accelerometer and gyroscope convergence + * + * @param[in] conv The IMU accelerometer and gyroscope convergence + * + * @returns the stringification of the IMU accelerometer and gyroscope convergence + */ +const char* FpaImuConvStr(const FpaImuConv conv); + /** * @brief FP_A GNSS fix status */ @@ -261,6 +324,15 @@ enum class FpaGnssStatus : int RESERVED9 = '9', //!< Reserved }; // clang-format on +/** + * @brief Stringify GNSS fix status + * + * @param[in] status TheGNSS fix status + * + * @returns the stringification of the GNSS fix status + */ +const char* FpaGnssStatusStr(const FpaGnssStatus status); + /** * @brief FP_A GNSS correction status */ @@ -279,6 +351,15 @@ enum class FpaCorrStatus : int RESERVED9 = '9', //!< Reserved }; // clang-format on +/** + * @brief Stringify GNSS correction status + * + * @param[in] status The GNSS correction status + * + * @returns the stringification of the GNSS correction status + */ +const char* FpaCorrStatusStr(const FpaCorrStatus status); + /** * @brief FP_A baseline status */ @@ -291,6 +372,15 @@ enum class FpaBaselineStatus : int PASSING = '3', //!< Passing }; // clang-format on +/** + * @brief Stringify baseline status + * + * @param[in] status The baseline status + * + * @returns the stringification of the baseline status + */ +const char* FpaBaselineStatusStr(const FpaBaselineStatus status); + /** * @brief FP_A camera status */ @@ -305,6 +395,15 @@ enum class FpaCamStatus : int GOOD = '5', //!< Camera working and available }; // clang-format on +/** + * @brief Stringify camera status + * + * @param[in] status The camera status + * + * @returns the stringification of the camera status + */ +const char* FpaCamStatusStr(const FpaCamStatus status); + /** * @brief FP_A wheelspeed status */ @@ -319,7 +418,16 @@ enum class FpaWsStatus : int }; // clang-format on /** - * @brief FP_A Legacy wheelspeed status + * @brief Stringify wheelspeed status + * + * @param[in] status The wheelspeed status + * + * @returns the stringification of the wheelspeed status + */ +const char* FpaWsStatusStr(const FpaWsStatus status); + +/** + * @brief FP_A legacy wheelspeed status */ enum class FpaWsStatusLegacy : int { // clang-format off @@ -329,6 +437,15 @@ enum class FpaWsStatusLegacy : int ONE_OR_MORE_CONVERGED = '1', //!< At least one converged }; // clang-format on +/** + * @brief Stringify legacy wheelspeed status + * + * @param[in] status The legacy wheelspeed status + * + * @returns the stringification of the legacy wheelspeed status + */ +const char* FpaWsStatusLegacyStr(const FpaWsStatusLegacy status); + /** * @brief FP_A wheelspeed convergence status */ @@ -344,6 +461,15 @@ enum class FpaWsConv : int IDLE = '6', //!< Idle }; // clang-format on +/** + * @brief Stringify wheelspeed convergence status + * + * @param[in] status The wheelspeed convergence status + * + * @returns the stringification of the wheelspeed convergence status + */ +const char* FpaWsConvStr(const FpaWsConv status); + /** * @brief FP_A markers status */ @@ -356,6 +482,15 @@ enum class FpaMarkersStatus : int ALL_CONVERGED = '3', //!< All markers available and used }; // clang-format on +/** + * @brief Stringify markers status + * + * @param[in] status The markers status + * + * @returns the stringification of the markers status + */ +const char* FpaMarkersStatusStr(const FpaMarkersStatus status); + /** * @brief FP_A markers convergence status */ @@ -369,6 +504,15 @@ enum class FpaMarkersConv : int IDLE = '4', //!< Idle }; // clang-format on +/** + * @brief Stringify markers convergence status + * + * @param[in] conv The markers convergence status + * + * @returns the stringification of the markers convergence status + */ +const char* FpaMarkersConvStr(const FpaMarkersConv conv); + /** * @brief FP_A GNSS fix type */ @@ -386,6 +530,15 @@ enum class FpaGnssFix : int RTK_FIXED = '8', //!< RTK fixed fix }; // clang-format on +/** + * @brief Stringify fix type + * + * @param[in] fix The fix type + * + * @returns the stringification of the fix type ("S3D", "RTK_FLOAT", etc.) + */ +const char* FpaGnssFixStr(const FpaGnssFix fix); + /** * @brief FP_A epoch type */ @@ -418,6 +571,15 @@ enum class FpaAntState : int SHORT, //!< Antenna short circuit detected }; +/** + * @brief Stringify antenna state + * + * @param[in] state The antenna state + * + * @returns the stringification of the antenna state ("OPEN", "OK", etc.) + */ +const char* FpaAntStateStr(const FpaAntState state); + /** * @brief FP_A GNSS antenna power */ @@ -428,6 +590,15 @@ enum class FpaAntPower : int OFF, //!< Antenna power supply is off }; +/** + * @brief Stringify antenna power + * + * @param[in] power The antenna power + * + * @returns the stringification of the antenna power ("ON", "OFF", etc.) + */ +const char* FpaAntPowerStr(const FpaAntPower power); + /** * @brief FP_A text levels */ @@ -439,6 +610,16 @@ enum class FpaTextLevel : int INFO = types::EnumToVal(logging::LoggingLevel::INFO), //!< Info DEBUG = types::EnumToVal(logging::LoggingLevel::DEBUG), //!< Debug }; // clang-format on + +/** + * @brief Stringify text level + * + * @param[in] level The text level + * + * @returns the stringification of the text level ("ERROR", "INFO", etc.) + */ +const char* FpaTextLevelStr(const FpaTextLevel level); + /** * @brief FP_A time base */ @@ -450,6 +631,15 @@ enum class FpaTimebase : int UTC, //!< UTC }; // clang-format on +/** + * @brief Stringify time base + * + * @param[in] base The time base + * + * @returns the stringification of the time base ("GNSS", "UTC", etc.) + */ +const char* FpaTimebaseStr(const FpaTimebase base); + /** * @brief FP_A time reference */ @@ -472,6 +662,15 @@ enum class FpaTimeref : int OTHER, //!< other/unknown GNSS/UTC }; // clang-format on +/** + * @brief Stringify time reference + * + * @param[in] ref The time reference + * + * @returns the stringification of the time reference ("UTC_NONE", "GNSS_GPS", etc.) + */ +const char* FpaTimerefStr(const FpaTimeref ref); + /** * @brief FP_A integer value */ @@ -653,14 +852,6 @@ struct FpaGnsscorrPayload : public FpaPayload */ struct FpaImuPayload : public FpaPayload { - //! Data from which FP_A-...IMU is stored - enum class Which - { - UNSPECIFIED, //!< Unspecified - RAWIMU, //!< Data is from FP_A-RAWIMU - CORRIMU, //!< Data is from FP_A-CORRIMU - }; - Which which = Which::UNSPECIFIED; //!< Indicates from which message the data is // clang-format off FpaGpsTime gps_time; //!< Time FpaFloat3 acc; //!< Raw acceleration in output frame, X/Y/Z components @@ -731,16 +922,7 @@ struct FpaLlhPayload : public FpaPayload */ struct FpaOdomPayload : public FpaPayload { - //! Data from which FP_A-...IMU is stored - enum class Which - { - UNSPECIFIED, //!< Unspecified - ODOMETRY, //!< Data is from FP_A-ODOMETRY - ODOMENU, //!< Data is from FP_A-ODOMENU - ODOMSH, //!< Data is from FP_A-ODOMSH - }; // clang-format off - Which which = Which::UNSPECIFIED; //!< Indicates from which message the data is FpaGpsTime gps_time; //!< Time FpaFloat3 pos; //!< Position, X/Y/Z components FpaFloat4 orientation; //!< Quaternion, W/X/Y/Z components diff --git a/fpsdk_common/include/fpsdk_common/parser/nmea.hpp b/fpsdk_common/include/fpsdk_common/parser/nmea.hpp index d02acb15..61dd68dd 100644 --- a/fpsdk_common/include/fpsdk_common/parser/nmea.hpp +++ b/fpsdk_common/include/fpsdk_common/parser/nmea.hpp @@ -222,6 +222,41 @@ enum class NmeaTalkerId : int GNSS = 'N', //!< GNSS (multi-constellation) }; // clang-format on +/** + * @brief Stringify NMEA talker ID + * + * @param[in] talker The NMEA talker ID + * + * @returns the stringification of the NMEA talker ID + */ +const char* NmeaTalkerIdStr(const NmeaTalkerId talker); + +/** + * @brief NMEA formatter + */ +enum class NmeaFormatter +{ + UNSPECIFIED, //!< Unspecified + GGA, //!< Formatter GGA (NmeaGgaPayload) + GLL, //!< Formatter GLL (NmeaGllPayload) + RMC, //!< Formatter RMC (NmeaRmcPayload) + VTG, //!< Formatter VTG (NmeaVtgPayload) + GST, //!< Formatter GST (NmeaGstPayload) + HDT, //!< Formatter HDT (NmeaHdtPayload) + ZDA, //!< Formatter ZDA (NmeaZdaPayload) + GSA, //!< Formatter GSA (NmeaGsaPayload) + GSV, //!< Formatter GSV (NmeaGsvPayload) +}; + +/** + * @brief Stringify NMEA formatter + * + * @param[in] formatter The NMEA formatter + * + * @returns the stringification of the NMEA formatter + */ +const char* NmeaFormatterStr(const NmeaFormatter formatter); + /** * @brief NMEA-Gx-GGA quality indicator */ @@ -240,7 +275,16 @@ enum class NmeaQualityGga : int }; // clang-format on /** - * @brief NMEA-Gx-GLL and NMEA-Gx-RMC status + * @brief Stringify NMEA-Gx-GGA quality indicator + * + * @param[in] qual The NMEA-Gx-GGA quality indicator + * + * @returns the stringification of the NMEA-Gx-GGA quality indicator + */ +const char* NmeaQualityGgaStr(const NmeaQualityGga qual); + +/** + * @brief NMEA-Gx-GLL and NMEA-Gx-RMC status * * @note Do not use <, >, >=, <= operators on this! */ @@ -252,6 +296,15 @@ enum class NmeaStatusGllRmc : int // DIFFERENTIAL = 'D', // @todo another possible value? }; // clang-format on +/** + * @brief Stringify NMEA-Gx-GLL and NMEA-Gx-RMC status + * + * @param[in] status The NMEA-Gx-GLL and NMEA-Gx-RMC status + * + * @returns the stringification of the NMEA-Gx-GLL and NMEA-Gx-RMC status + */ +const char* NmeaStatusGllRmcStr(const NmeaStatusGllRmc status); + /** * @brief NMEA-Gx-GLL and NMEA-Gx-VTG pos mode * @@ -269,7 +322,16 @@ enum class NmeaModeGllVtg : int }; // clang-format on /** - * @brief NMEA-Gx-RMC and NMEA-Gx-GNS pos mode + * @brief Stringify NMEA-Gx-GLL and NMEA-Gx-VTG pos mode + * + * @param[in] mode The NMEA-Gx-GLL and NMEA-Gx-VTG pos mode + * + * @returns the stringification of the NMEA-Gx-GLL and NMEA-Gx-VTG pos mode + */ +const char* NmeaModeGllVtgStr(const NmeaModeGllVtg mode); + +/** + * @brief NMEA-Gx-RMC and NMEA-Gx-GNS pos mode * * @note Do not use <, >, >=, <= operators on this! */ @@ -287,6 +349,15 @@ enum class NmeaModeRmcGns : int SIM = 'S', //!< Simulator mode }; // clang-format on +/** + * @brief Stringify NMEA-Gx-RMC and NMEA-Gx-GNS pos mode + * + * @param[in] mode The NMEA-Gx-RMC and NMEA-Gx-GNS pos mode + * + * @returns the stringification of the NMEA-Gx-RMC and NMEA-Gx-GNS pos mode + */ +const char* NmeaModeRmcGnsStr(const NmeaModeRmcGns mode); + /** * @brief NMEA-Gx-RMC navigational status * @@ -301,6 +372,15 @@ enum class NmeaNavStatusRmc : int NA = 'V', //!< Equipment does not provide navigational status }; // clang-format on +/** + * @brief Stringify NMEA-Gx-RMC navigational status + * + * @param[in] navstatus The NMEA-Gx-RMC navigational status + * + * @returns the stringification of the NMEA-Gx-RMC navigational status + */ +const char* NmeaNavStatusRmcStr(const NmeaNavStatusRmc navstatus); + /** * @brief NMEA-Gx-GNS operation mode * @@ -313,6 +393,15 @@ enum class NmeaOpModeGsa : int AUTO = 'A', //!< Automatic }; // clang-format on +/** + * @brief Stringify NMEA-Gx-GNS operation mode + * + * @param[in] opmode The NMEA-Gx-GNS operation mode + * + * @returns the stringification of the NMEA-Gx-GNS operation mode + */ +const char* NmeaOpModeGsaStr(const NmeaOpModeGsa opmode); + /** * @brief NMEA-Gx-GNS nav mode */ @@ -324,6 +413,15 @@ enum class NmeaNavModeGsa : int FIX3D = '3', //!< 3D fix }; // clang-format on +/** + * @brief Stringify NMEA-Gx-GNS nav mode + * + * @param[in] navmode The NMEA-Gx-GNS nav mode + * + * @returns the stringification of the NMEA-Gx-GNS nav mode + */ +const char* NmeaNavModeGsaStr(const NmeaNavModeGsa navmode); + /** * @brief NMEA system IDs * @@ -340,6 +438,15 @@ enum class NmeaSystemId : int NAVIC = '6', //!< NavIC }; // clang-format on +/** + * @brief Stringify NMEA system ID + * + * @param[in] system The NMEA system ID + * + * @returns the stringification of the NMEA system ID + */ +const char* NmeaSystemIdStr(const NmeaSystemId system); + /** * @brief NMEA signal IDs * @@ -349,32 +456,47 @@ enum class NmeaSignalId : int { // clang-format off UNSPECIFIED = 0x000 + '!', //!< Unspecified NONE = 0x000 + '0', //!< None + // GPS (SBAS) GPS_L1CA = 0x100 + '1', //!< GPS L1 C/A or SBAS L1 C/A GPS_L2CL = 0x100 + '6', //!< GPS L2 CL GPS_L2CM = 0x100 + '5', //!< GPS L2 CM GPS_L5I = 0x100 + '7', //!< GPS L5 I GPS_L5Q = 0x100 + '8', //!< GPS L5 Q - GLO_L1OF = 0x200 + '1', //!< GLONASS L1 OF - GLO_L2OF = 0x200 + '3', //!< GLONASS L2 OF + // GAL GAL_E1 = 0x300 + '7', //!< Galileo E1 GAL_E5A = 0x300 + '1', //!< Galileo E5 A GAL_E5B = 0x300 + '2', //!< Galileo E5 B GAL_E6BC = 0x300 + '3', //!< Galileo E6 B/C GAL_E6A = 0x300 + '4', //!< Galileo E6 A + // BDS BDS_B1ID = 0x400 + '1', //!< BeiDou B1I D BDS_B2ID = 0x400 + 'B', //!< BeiDou B2I D BDS_B1C = 0x400 + '3', //!< BeiDou B1 C BDS_B2A = 0x400 + '5', //!< BeiDou B2 a BDS_B2B = 0x400 + '6', //!< BeiDou B2 b + // QZSS QZSS_L1CA = 0x500 + '1', //!< QZSS L1 C/A QZSS_L1S = 0x500 + '4', //!< QZSS L1S QZSS_L2CM = 0x500 + '5', //!< QZSS L2 CM QZSS_L2CL = 0x500 + '6', //!< QZSS L2 CL QZSS_L5I = 0x500 + '7', //!< QZSS L5 I QZSS_L5Q = 0x500 + '8', //!< QZSS L5 Q + // GLO + GLO_L1OF = 0x200 + '1', //!< GLONASS L1 OF + GLO_L2OF = 0x200 + '3', //!< GLONASS L2 OF + // NAVIC NAVIC_L5A = 0x600 + '5', //!< NavIC L5 A }; // clang-format on +/** + * @brief Stringify NMEA signal IDs + * + * @param[in] signal The NMEA signal IDs + * + * @returns the stringification of the NMEA signal IDs + */ +const char* NmeaSignalIdStr(const NmeaSignalId signal); + /** * @brief NMEA time (hour, minutes, seconds) */ @@ -491,23 +613,6 @@ struct NmeaFloat double value = 0; //!< Value }; -/** - * @brief NMEA formatter - */ -enum class NmeaFormatter -{ - UNSPECIFIED, //!< Unspecified - GGA, //!< Formatter GGA (NmeaGgaPayload) - GLL, //!< Formatter GLL (NmeaGllPayload) - RMC, //!< Formatter RMC (NmeaRmcPayload) - VTG, //!< Formatter VTG (NmeaVtgPayload) - GST, //!< Formatter GST (NmeaGstPayload) - HDT, //!< Formatter HDT (NmeaHdtPayload) - ZDA, //!< Formatter ZDA (NmeaZdaPayload) - GSA, //!< Formatter GSA (NmeaGsaPayload) - GSV, //!< Formatter GSV (NmeaGsvPayload) -}; - /** * @brief NMEA payload base class */ diff --git a/fpsdk_common/include/fpsdk_common/path.hpp b/fpsdk_common/include/fpsdk_common/path.hpp index f931f54e..b4110180 100644 --- a/fpsdk_common/include/fpsdk_common/path.hpp +++ b/fpsdk_common/include/fpsdk_common/path.hpp @@ -28,6 +28,7 @@ /* EXTERNAL */ /* PACKAGE */ +#include "types.hpp" namespace fpsdk { namespace common { @@ -109,10 +110,28 @@ bool PathIsExecutable(const std::string& path); */ std::size_t FileSize(const std::string& path); +/** + * @brief Get directory size + * + * @param[in] path Path to directory + * + * @returns the size of all the files in the directory in bytes, 0 is only valid if file is readable + */ +std::size_t DirSize(const std::string& path); + +/** + * @brief Remove, recursively + * + * Removes the path, including all its subdirectories, recursively. + * + * @param[in] path The path to remove + */ +void RemoveAll(const std::string& path); + /** * @brief Output file handle */ -class OutputFile +class OutputFile : private types::NoCopyNoMove { public: OutputFile(); @@ -151,6 +170,15 @@ class OutputFile */ bool Write(const uint8_t* data, const std::size_t size); + /** + * @brief Write data to file + * + * @param[in] data The data to be written + * + * @returns true on success, false otherwise + */ + bool Write(const std::string& data); + /** * @brief Get file path * diff --git a/fpsdk_common/include/fpsdk_common/ros1.hpp b/fpsdk_common/include/fpsdk_common/ros1.hpp new file mode 100644 index 00000000..6e01c32d --- /dev/null +++ b/fpsdk_common/include/fpsdk_common/ros1.hpp @@ -0,0 +1,130 @@ +/** + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ + * \endverbatim + * + * @file + * @brief Fixposition SDK: ROS1 types and utils + * + * @page FPSDK_COMMON_ROS1 ROS1 types and utils + * + * **API**: fpsdk_common/ros1.hpp and fpsdk::common::ros1 + * + */ +#ifndef __FPSDK_COMMON_ROS1_HPP__ +#define __FPSDK_COMMON_ROS1_HPP__ + +/* LIBC/STL */ +#include +#include + +/* EXTERNAL */ +#include + +/* ROS */ +#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" +// +#include +#include +#include +#include +#include +// +#include +// +#pragma GCC diagnostic pop + +/* PACKAGE */ + +namespace fpsdk { +namespace common { +/** + * @brief ROS1 types and utils + */ +namespace ros1 { +/* ****************************************************************************************************************** */ + +#ifndef _DOXYGEN_ +// Helper for ros de-serialization. We could DeserializeMessage() using ros::serialization::deserialize(IStream(buf), +// msg). However, the IStream() wants a mutable buffer, which is not nice. ConstBuffer() provides the Stream() interface +// without needing the mutability of the buffer itself. +struct ConstBuffer +{ + ConstBuffer(const std::vector& buf) + : data_{ buf.data() }, end_{ buf.data() + static_cast(buf.size()) } + { + } + ConstBuffer(const uint8_t* data, size_t size) : data_(data), end_(data + static_cast(size)) + { + } + + static const ros::serialization::StreamType stream_type = ros::serialization::stream_types::Input; + inline const uint8_t* getData() + { + return data_; + } + inline const uint8_t* advance(uint32_t len) + { + const uint8_t* old_data = data_; + data_ += len; + if (data_ > end_) { + ros::serialization::throwStreamOverrun(); + } + return old_data; + } + inline uint32_t getLength() + { + return static_cast(end_ - data_); + } + + template + inline void next(T& t) + { + ros::serialization::deserialize(*this, t); + } + + template + inline ConstBuffer& operator>>(T& t) + { + ros::serialization::deserialize(*this, t); + return *this; + } + + private: + const uint8_t* data_; + const uint8_t* end_; +}; +#endif // _DOXYGEN_ + +/** + * @brief Deserialise ROS1 message + * + * @tparam RosMsgT The ROS1 message type + * @param[in] buf The serialised ROS message + * @param[out] msg The deserialised ROS message + */ +template +inline void DeserializeMessage(const std::vector& buf, RosMsgT& msg) +{ +#if FPSDK_USE_ROS1 + ros::serialization::IStream s((uint8_t*)buf.data(), static_cast(buf.size())); // :-( + ros::serialization::deserialize(s, msg); +#else + ConstBuffer m(buf); + ros::serialization::Serializer::read(m, msg); +#endif +} + +/* ****************************************************************************************************************** */ +} // namespace ros1 +} // namespace common +} // namespace fpsdk +#endif // __FPSDK_COMMON_ROS1_HPP__ diff --git a/fpsdk_common/include/fpsdk_common/thread.hpp b/fpsdk_common/include/fpsdk_common/thread.hpp index 84aa701c..1e4e46ad 100644 --- a/fpsdk_common/include/fpsdk_common/thread.hpp +++ b/fpsdk_common/include/fpsdk_common/thread.hpp @@ -86,7 +86,7 @@ class BinarySemaphore * longer than one period in order to achieve the minimal wait duration and still timeout on the start of a period. * * Note that this works on the actual system clock. In ROS replay scenario this may not behave as expected as the - * fake ROS system clock may be faster or slower than the actual system clock. + * simulated ROS system clock may be faster or slower than the actual system clock. * * @param[in] period Period duration [ms], must be > 0 * @param[in] min_sleep Minimal sleep duration [ms], must be < period diff --git a/fpsdk_common/include/fpsdk_common/to_json/fpl.hpp b/fpsdk_common/include/fpsdk_common/to_json/fpl.hpp new file mode 100644 index 00000000..aa874969 --- /dev/null +++ b/fpsdk_common/include/fpsdk_common/to_json/fpl.hpp @@ -0,0 +1,112 @@ +/** + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ + * \endverbatim + * + * @file + * @brief Fixposition SDK: to_json() helpers for some fpsdk::common::fpl messages + */ +#ifndef __FPSDK_COMMON_TO_JSON_FPL_HPP__ +#define __FPSDK_COMMON_TO_JSON_FPL_HPP__ + +/* LIBC/STL */ + +/* EXTERNAL */ +#include + +/* PACKAGE */ +#include "../fpl.hpp" +#include "../string.hpp" +#include "time.hpp" + +#ifndef _DOXYGEN_ // not documenting these +/* ****************************************************************************************************************** */ +namespace fpsdk::common::fpl { + +inline void to_json(nlohmann::json& j, const LogStatus& s) +{ + j = nlohmann::json::object({ + { "_type", FplTypeStr(FplType::LOGSTATUS) }, + { "_yaml", s.yaml_ }, + { "state", s.state_ }, + { "queue_size_", s.queue_size_ }, + { "queue_peak", s.queue_peak_ }, + { "queue_skip", s.queue_skip_ }, + { "queue_bsize", s.queue_bsize_ }, + { "queue_bpeak", s.queue_bpeak_ }, + { "log_count", s.log_count_ }, + { "log_errors", s.log_errors_ }, + { "log_size", s.log_size_ }, + { "log_duration", s.log_duration_ }, + { "log_time_posix", s.log_time_posix_ }, + { "log_time_iso", s.log_time_iso_ }, + { "pos_source", s.pos_source_ }, + { "pos_fix_type", s.pos_fix_type_ }, + { "pos_lat", s.pos_lat_ }, + { "pos_lon", s.pos_lon_ }, + { "pos_height", s.pos_height_ }, + }); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const LogMeta& m) +{ + j = nlohmann::json::object({ + { "_type", FplTypeStr(FplType::LOGMETA) }, + { "_yaml", m.yaml_ }, + { "hw_uid", m.hw_uid_ }, + { "product_model", m.product_model_ }, + { "sw_version", m.sw_version_ }, + { "log_start_time_posix", m.log_start_time_posix_ }, + { "log_start_time_iso", m.log_start_time_iso_ }, + { "log_profile", m.log_profile_ }, + { "log_target", m.log_target_ }, + { "log_filename", m.log_filename_ }, + }); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const StreamMsg& m) +{ + j = nlohmann::json::object({ + { "_type", FplTypeStr(FplType::STREAMMSG) }, + { "_stamp", m.rec_time_ }, + { "_stream", m.stream_name_ }, + }); + + if (std::all_of(m.msg_data_.data(), m.msg_data_.data() + m.msg_data_.size(), + [](const uint8_t c) { return std::isprint(c) || std::isspace(c); })) { + j["_raw"] = string::BufToStr(m.msg_data_); + } else { + j["_raw_b64"] = string::Base64Enc(m.msg_data_); + } +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const FileDump& m) +{ + j = nlohmann::json::object({ + { "_type", FplTypeStr(FplType::FILEDUMP) }, + { "_mtime", m.mtime_.GetRosTime() }, + { "_filename", m.filename_ }, + }); + + if (std::all_of(m.data_.data(), m.data_.data() + m.data_.size(), + [](const uint8_t c) { return std::isprint(c) || std::isspace(c); })) { + j["_data"] = string::BufToStr(m.data_); + } else { + j["_data_b64"] = string::Base64Enc(m.data_); + } +} + +} // namespace fpsdk::common::fpl +/* ****************************************************************************************************************** */ +#endif // !_DOXYGEN_ +#endif // __FPSDK_COMMON_TO_JSON_FPL_HPP__ diff --git a/fpsdk_common/include/fpsdk_common/to_json/fpl_ros1.hpp b/fpsdk_common/include/fpsdk_common/to_json/fpl_ros1.hpp new file mode 100644 index 00000000..e8566788 --- /dev/null +++ b/fpsdk_common/include/fpsdk_common/to_json/fpl_ros1.hpp @@ -0,0 +1,46 @@ +/** + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ + * \endverbatim + * + * @file + * @brief Fixposition SDK: to_json() helpers for some fpsdk::common::fpl::RosMsgBin + */ +#ifndef __FPSDK_COMMON_TO_JSON_FPL_ROS1_HPP__ +#define __FPSDK_COMMON_TO_JSON_FPL_ROS1_HPP__ + +/* LIBC/STL */ + +/* EXTERNAL */ +#include + +/* PACKAGE */ +#include "../fpl.hpp" +#include "../ros1.hpp" +#include "../string.hpp" +#include "ros1.hpp" + +#ifndef _DOXYGEN_ // not documenting these +/* ****************************************************************************************************************** */ +namespace fpsdk::common::fpl { + +template +inline bool RosMsgToJson(const RosMsgDef& def, const RosMsgBin& bin, nlohmann::json& json) +{ + if (def.msg_name_ == ros::message_traits::datatype()) { + RosMsgT msg; + ros1::DeserializeMessage(bin.msg_data_, msg); // This can throw! + json = msg; + return true; + } + return false; +} + +} // namespace fpsdk::common::fpl +/* ****************************************************************************************************************** */ +#endif // !_DOXYGEN_ +#endif // __FPSDK_COMMON_TO_JSON_FPL_ROS1_HPP__ diff --git a/fpsdk_common/include/fpsdk_common/to_json/nmea.hpp b/fpsdk_common/include/fpsdk_common/to_json/nmea.hpp new file mode 100644 index 00000000..9b9a6afe --- /dev/null +++ b/fpsdk_common/include/fpsdk_common/to_json/nmea.hpp @@ -0,0 +1,339 @@ +/** + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ + * \endverbatim + * + * @file + * @brief Fixposition SDK: to_json() helpers for some fpsdk::common::parser::nmea messages + */ +#ifndef __FPSDK_COMMON_TO_JSON_NMEA_HPP__ +#define __FPSDK_COMMON_TO_JSON_NMEA_HPP__ + +/* LIBC/STL */ + +/* EXTERNAL */ +#include + +/* PACKAGE */ +#include "../parser/nmea.hpp" + +#ifndef _DOXYGEN_ // not documenting these +/* ****************************************************************************************************************** */ +namespace fpsdk::common::parser::nmea { + +inline void to_json(nlohmann::json& j, const NmeaTalkerId e) +{ + j = NmeaTalkerIdStr(e); +} + +inline void to_json(nlohmann::json& j, const NmeaFormatter e) +{ + j = NmeaFormatterStr(e); +} + +inline void to_json(nlohmann::json& j, const NmeaQualityGga e) +{ + j = NmeaQualityGgaStr(e); +} + +inline void to_json(nlohmann::json& j, const NmeaStatusGllRmc e) +{ + j = NmeaStatusGllRmcStr(e); +} + +inline void to_json(nlohmann::json& j, const NmeaModeGllVtg e) +{ + j = NmeaModeGllVtgStr(e); +} + +inline void to_json(nlohmann::json& j, const NmeaModeRmcGns e) +{ + j = NmeaModeRmcGnsStr(e); +} + +inline void to_json(nlohmann::json& j, const NmeaNavStatusRmc e) +{ + j = NmeaNavStatusRmcStr(e); +} + +inline void to_json(nlohmann::json& j, const NmeaOpModeGsa e) +{ + j = NmeaOpModeGsaStr(e); +} + +inline void to_json(nlohmann::json& j, const NmeaNavModeGsa e) +{ + j = NmeaNavModeGsaStr(e); +} + +inline void to_json(nlohmann::json& j, const NmeaSystemId e) +{ + j = NmeaSystemIdStr(e); +} + +inline void to_json(nlohmann::json& j, const NmeaSignalId e) +{ + j = NmeaSignalIdStr(e); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const NmeaTime& m) +{ + if (m.valid) { + j = nlohmann::json::object({ + { "hours", m.hours }, + { "mins", m.mins }, + { "secs", m.secs }, + }); + } else { + j = nullptr; + } +} + +inline void to_json(nlohmann::json& j, const NmeaDate& m) +{ + if (m.valid) { + j = nlohmann::json::object({ + { "years", m.years }, + { "months", m.months }, + { "days", m.days }, + }); + } else { + j = nullptr; + } +} + +inline void to_json(nlohmann::json& j, const NmeaLlh& m) +{ + if (m.latlon_valid) { + j = nlohmann::json::object({ + { "lat", m.lat }, + { "lon", m.lon }, + }); + if (m.height_valid) { + j["height"] = m.height; + } + } else { + j = nullptr; + } +} + +inline void to_json(nlohmann::json& j, const NmeaSat& m) +{ + if (m.valid) { + j = nlohmann::json::object({ + { "system", m.system }, + { "svid", m.svid }, + }); + } else { + j = nullptr; + } +} + +inline void to_json(nlohmann::json& j, const NmeaAzEl& m) +{ + if (m.valid) { + j = nlohmann::json::object({ + { "system", m.system }, + { "svid", m.svid }, + { "el", m.el }, + { "az", m.az }, + }); + } else { + j = nullptr; + } +} + +inline void to_json(nlohmann::json& j, const NmeaCno& m) +{ + if (m.valid) { + j = nlohmann::json::object({ + { "system", m.system }, + { "svid", m.svid }, + { "signal", m.signal }, + { "cno", m.cno }, + }); + } else { + j = nullptr; + } +} + +inline void to_json(nlohmann::json& j, const NmeaInt& m) +{ + if (m.valid) { + j = m.value; + } else { + j = nullptr; + } +} + +inline void to_json(nlohmann::json& j, const NmeaFloat& m) +{ + if (m.valid) { + j = m.value; + } else { + j = nullptr; + } +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const NmeaGgaPayload& m) +{ + j = nlohmann::json::object({ + { "time", m.time }, + { "llh", m.llh }, + { "height_msl", m.height_msl }, + { "quality", m.quality }, + { "num_sv", m.num_sv }, + { "hdop", m.hdop }, + { "diff_age", m.diff_age }, + { "diff_sta", m.diff_sta }, + }); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const NmeaGllPayload& m) +{ + j = nlohmann::json::object({ + { "ll", m.ll }, + { "time", m.time }, + { "status", m.status }, + { "mode", m.mode }, + }); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const NmeaRmcPayload& m) +{ + j = nlohmann::json::object({ + { "time", m.time }, + { "ll", m.ll }, + { "speed", m.speed }, + { "course", m.course }, + { "date", m.date }, + { "mode", m.mode }, + { "navstatus", m.navstatus }, + }); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const NmeaVtgPayload& m) +{ + j = nlohmann::json::object({ + { "cogt", m.cogt }, + { "cogm", m.cogm }, + { "sogn", m.sogn }, + { "sogk", m.sogk }, + { "mode", m.mode }, + }); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const NmeaGstPayload& m) +{ + j = nlohmann::json::object({ + { "time", m.time }, + { "rms_range", m.rms_range }, + { "std_major", m.std_major }, + { "std_minor", m.std_minor }, + { "angle_major", m.angle_major }, + { "std_lat", m.std_lat }, + { "std_lon", m.std_lon }, + { "std_alt", m.std_alt }, + }); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const NmeaHdtPayload& m) +{ + j = nlohmann::json::object({ + { "heading", m.heading }, + }); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const NmeaZdaPayload& m) +{ + j = nlohmann::json::object({ + { "time", m.time }, + { "date", m.date }, + { "local_hr", m.local_hr }, + { "local_min", m.local_min }, + }); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const NmeaGsaPayload& m) +{ + j = nlohmann::json::object({ + { "opmode", m.opmode }, + { "navmode", m.navmode }, + { "sats", m.sats }, + { "num_sats", m.num_sats }, + { "pdop", m.pdop }, + { "hdop", m.hdop }, + { "vdop", m.vdop }, + { "system", m.system }, + }); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const NmeaGsvPayload& m) +{ + j = nlohmann::json::object({ + { "num_msgs", m.num_msgs }, + { "msg_num", m.msg_num }, + { "tot_num_sat", m.tot_num_sat }, + { "azels", m.azels }, + { "num_azels", m.num_azels }, + { "num_cnos", m.num_cnos }, + { "system", m.system }, + { "signal", m.signal }, + }); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const NmeaPayloadPtr& nmea) +{ + j = nlohmann::json::object(); + if (!nmea) { + return; + } + j["_valid"] = nmea->valid_; + j["_talker"] = nmea->talker_; + j["_formatter"] = nmea->formatter_; + if (nmea) { + switch (nmea->formatter_) { // clang-format off + case NmeaFormatter::UNSPECIFIED: break; + case NmeaFormatter::GGA: j.update(dynamic_cast(*nmea)); break; + case NmeaFormatter::GLL: j.update(dynamic_cast(*nmea)); break; + case NmeaFormatter::RMC: j.update(dynamic_cast(*nmea)); break; + case NmeaFormatter::VTG: j.update(dynamic_cast(*nmea)); break; + case NmeaFormatter::GST: j.update(dynamic_cast(*nmea)); break; + case NmeaFormatter::HDT: j.update(dynamic_cast(*nmea)); break; + case NmeaFormatter::ZDA: j.update(dynamic_cast(*nmea)); break; + case NmeaFormatter::GSA: j.update(dynamic_cast(*nmea)); break; + case NmeaFormatter::GSV: j.update(dynamic_cast(*nmea)); break; + } // clang-format on + } +} + +} // namespace fpsdk::common::parser::nmea +/* ****************************************************************************************************************** */ +#endif // !_DOXYGEN_ +#endif // __FPSDK_COMMON_TO_JSON_NMEA_HPP__ diff --git a/fpsdk_common/include/fpsdk_common/to_json/parser.hpp b/fpsdk_common/include/fpsdk_common/to_json/parser.hpp new file mode 100644 index 00000000..39e4ac97 --- /dev/null +++ b/fpsdk_common/include/fpsdk_common/to_json/parser.hpp @@ -0,0 +1,53 @@ +/** + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ + * \endverbatim + * + * @file + * @brief Fixposition SDK: to_json() helpers for some fpsdk::common::parser + */ +#ifndef __FPSDK_COMMON_TO_JSON_PARSER_HPP__ +#define __FPSDK_COMMON_TO_JSON_PARSER_HPP__ + +/* LIBC/STL */ + +/* EXTERNAL */ +#include + +/* PACKAGE */ +#include "../parser/types.hpp" + +#ifndef _DOXYGEN_ // not documenting these +/* ****************************************************************************************************************** */ +namespace fpsdk::common::parser { + +inline void to_json(nlohmann::json& j, const ParserMsg& m) +{ + j = nlohmann::json::object({ + { "_proto", ProtocolStr(m.proto_) }, + { "_name", m.name_ }, + { "_seq", m.seq_ }, + }); + + if (!m.info_.empty()) { + j["_info"] = m.info_; + } else { + j["_info"] = nullptr; + } + + if (std::all_of(m.data_.data(), m.data_.data() + m.data_.size(), + [](const uint8_t c) { return std::isprint(c) || std::isspace(c); })) { + j["_data"] = string::BufToStr(m.data_); + } else { + j["_data_b64"] = string::Base64Enc(m.data_); + } +} + +} // namespace fpsdk::common::parser +/* ****************************************************************************************************************** */ +#endif // !_DOXYGEN_ +#endif // __FPSDK_COMMON_TO_JSON_PARSER_HPP__ diff --git a/fpsdk_common/include/fpsdk_common/to_json/parser_fpa.hpp b/fpsdk_common/include/fpsdk_common/to_json/parser_fpa.hpp new file mode 100644 index 00000000..083347f5 --- /dev/null +++ b/fpsdk_common/include/fpsdk_common/to_json/parser_fpa.hpp @@ -0,0 +1,452 @@ +/** + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ + * \endverbatim + * + * @file + * @brief Fixposition SDK: to_json() helpers for some fpsdk::common::parser::fpa messages + */ +#ifndef __FPSDK_COMMON_TO_JSON_PARSER_FPA_HPP__ +#define __FPSDK_COMMON_TO_JSON_PARSER_FPA_HPP__ + +/* LIBC/STL */ + +/* EXTERNAL */ +#include + +/* PACKAGE */ +#include "../parser/fpa.hpp" + +#ifndef _DOXYGEN_ // not documenting these +/* ****************************************************************************************************************** */ +namespace fpsdk::common::parser::fpa { + +inline void to_json(nlohmann::json& j, const FpaInitStatus e) +{ + j = FpaInitStatusStr(e); +} + +inline void to_json(nlohmann::json& j, const FpaFusionStatusLegacy e) +{ + j = FpaFusionStatusLegacyStr(e); +} + +inline void to_json(nlohmann::json& j, const FpaMeasStatus e) +{ + j = FpaMeasStatusStr(e); +} + +inline void to_json(nlohmann::json& j, const FpaImuStatus e) +{ + j = FpaImuStatusStr(e); +} + +inline void to_json(nlohmann::json& j, const FpaImuStatusLegacy e) +{ + j = FpaImuStatusLegacyStr(e); +} + +inline void to_json(nlohmann::json& j, const FpaImuNoise e) +{ + j = FpaImuNoiseStr(e); +} + +inline void to_json(nlohmann::json& j, const FpaImuConv e) +{ + j = FpaImuConvStr(e); +} + +inline void to_json(nlohmann::json& j, const FpaGnssStatus e) +{ + j = FpaGnssStatusStr(e); +} + +inline void to_json(nlohmann::json& j, const FpaCorrStatus e) +{ + j = FpaCorrStatusStr(e); +} + +inline void to_json(nlohmann::json& j, const FpaBaselineStatus e) +{ + j = FpaBaselineStatusStr(e); +} + +inline void to_json(nlohmann::json& j, const FpaCamStatus e) +{ + j = FpaCamStatusStr(e); +} + +inline void to_json(nlohmann::json& j, const FpaWsStatus e) +{ + j = FpaWsStatusStr(e); +} + +inline void to_json(nlohmann::json& j, const FpaWsStatusLegacy e) +{ + j = FpaWsStatusLegacyStr(e); +} + +inline void to_json(nlohmann::json& j, const FpaWsConv e) +{ + j = FpaWsConvStr(e); +} + +inline void to_json(nlohmann::json& j, const FpaMarkersStatus e) +{ + j = FpaMarkersStatusStr(e); +} + +inline void to_json(nlohmann::json& j, const FpaMarkersConv e) +{ + j = FpaMarkersConvStr(e); +} + +inline void to_json(nlohmann::json& j, const FpaGnssFix e) +{ + j = FpaGnssFixStr(e); +} + +inline void to_json(nlohmann::json& j, const FpaEpoch e) +{ + j = FpaEpochStr(e); +} + +inline void to_json(nlohmann::json& j, const FpaAntState e) +{ + j = FpaAntStateStr(e); +} + +inline void to_json(nlohmann::json& j, const FpaAntPower e) +{ + j = FpaAntPowerStr(e); +} + +inline void to_json(nlohmann::json& j, const FpaTextLevel e) +{ + j = FpaTextLevelStr(e); +} + +inline void to_json(nlohmann::json& j, const FpaTimebase e) +{ + j = FpaTimebaseStr(e); +} + +inline void to_json(nlohmann::json& j, const FpaTimeref e) +{ + j = FpaTimerefStr(e); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const FpaInt& m) +{ + if (m.valid) { + j = m.value; + } else { + j = nullptr; + } +} + +inline void to_json(nlohmann::json& j, const FpaFloat& m) +{ + if (m.valid) { + j = m.value; + } else { + j = nullptr; + } +} + +inline void to_json(nlohmann::json& j, const FpaFloat3& m) +{ + if (m.valid) { + j = m.values; + } else { + j = nullptr; + } +} + +inline void to_json(nlohmann::json& j, const FpaFloat4& m) +{ + if (m.valid) { + j = m.values; + } else { + j = nullptr; + } +} + +inline void to_json(nlohmann::json& j, const FpaFloat6& m) +{ + if (m.valid) { + j = m.values; + } else { + j = nullptr; + } +} + +inline void to_json(nlohmann::json& j, const FpaGpsTime& m) +{ + j = nlohmann::json::object({ + { "gps_week", m.week }, + { "gps_tow", m.tow }, + }); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const FpaEoePayload& m) +{ + j = nlohmann::json::object({ + { "epoch", m.epoch }, + }); + j.update(m.gps_time); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const FpaGnssantPayload& m) +{ + j = nlohmann::json::object({ + { "gnss1_state", m.gnss1_state }, + { "gnss1_power", m.gnss1_power }, + { "gnss1_age", m.gnss1_age }, + { "gnss2_state", m.gnss2_state }, + { "gnss2_power", m.gnss2_power }, + { "gnss2_age", m.gnss2_age }, + }); + j.update(m.gps_time); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const FpaGnsscorrPayload& m) +{ + j = nlohmann::json::object({ + { "gnss1_fix", m.gnss1_fix }, + { "gnss1_nsig_l1", m.gnss1_nsig_l1 }, + { "gnss1_nsig_l2", m.gnss1_nsig_l2 }, + { "gnss2_fix", m.gnss2_fix }, + { "gnss2_nsig_l2", m.gnss2_nsig_l1 }, + { "gnss2_nsig_l2", m.gnss2_nsig_l2 }, + { "corr_latency", m.corr_latency }, + { "corr_update_rate", m.corr_update_rate }, + { "corr_data_rate", m.corr_data_rate }, + { "corr_msg_rate", m.corr_msg_rate }, + { "sta_id", m.sta_id }, + { "sta_llh", m.sta_llh }, + { "sta_dist", m.sta_dist }, + }); + j.update(m.gps_time); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const FpaImuPayload& m) +{ + j = nlohmann::json::object({ + { "acc", m.acc }, + { "rot", m.rot }, + { "bias_comp", m.bias_comp }, + { "imu_status", m.imu_status }, + }); + j.update(m.gps_time); +} + +inline void to_json(nlohmann::json& j, const FpaRawimuPayload& m) +{ + to_json(j, dynamic_cast(m)); +} + +inline void to_json(nlohmann::json& j, const FpaCorrimuPayload& m) +{ + to_json(j, dynamic_cast(m)); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const FpaImubiasPayload& m) +{ + j = nlohmann::json::object({ + { "fusion_imu", m.fusion_imu }, + { "imu_status", m.imu_status }, + { "imu_noise", m.imu_noise }, + { "imu_conv", m.imu_conv }, + { "bias_acc", m.bias_acc }, + { "bias_gyr", m.bias_gyr }, + { "bias_cov_acc", m.bias_cov_acc }, + { "bias_cov_gyr", m.bias_cov_gyr }, + }); + j.update(m.gps_time); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const FpaLlhPayload& m) +{ + j = nlohmann::json::object({ + { "llh", m.llh }, + { "longitude", nullptr }, + { "height", nullptr }, + { "cov_enu", m.cov_enu }, + }); + j.update(m.gps_time); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const FpaOdomPayload& m) +{ + j = nlohmann::json::object({ + { "pos", m.pos }, + { "orientation", m.orientation }, + { "vel", m.vel }, + { "rot", m.rot }, + { "acc", m.acc }, + { "fusion_status", m.fusion_status }, + { "imu_bias_status", m.imu_bias_status }, + { "gnss1_fix", m.gnss1_fix }, + { "gnss2_fix", m.gnss2_fix }, + { "wheelspeed_status", m.wheelspeed_status }, + { "pos_cov", m.pos_cov }, + { "orientation_cov", m.orientation_cov }, + { "vel_cov", m.vel_cov }, + }); + j.update(m.gps_time); +} + +inline void to_json(nlohmann::json& j, const FpaOdometryPayload& m) +{ + if (m.valid_) { + to_json(j, dynamic_cast(m)); + j["version"] = m.version; + } +} + +inline void to_json(nlohmann::json& j, const FpaOdomenuPayload& m) +{ + to_json(j, dynamic_cast(m)); +} + +inline void to_json(nlohmann::json& j, const FpaOdomshPayload& m) +{ + to_json(j, dynamic_cast(m)); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const FpaOdomstatusPayload& m) +{ + j = nlohmann::json::object({ + { "init_status", m.init_status }, + { "fusion_imu", m.fusion_imu }, + { "fusion_gnss1", m.fusion_gnss1 }, + { "fusion_gnss2", m.fusion_gnss2 }, + { "fusion_corr", m.fusion_corr }, + { "fusion_cam1", m.fusion_cam1 }, + { "fusion_ws", m.fusion_ws }, + { "fusion_markers", m.fusion_markers }, + { "imu_status", m.imu_status }, + { "imu_noise", m.imu_noise }, + { "imu_conv", m.imu_conv }, + { "gnss1_status", m.gnss1_status }, + { "gnss2_status", m.gnss2_status }, + { "baseline_status", m.baseline_status }, + { "corr_status", m.corr_status }, + { "cam1_status", m.cam1_status }, + { "ws_status", m.ws_status }, + { "ws_conv", m.ws_conv }, + { "markers_status", m.markers_status }, + { "markers_conv", m.markers_conv }, + }); + j.update(m.gps_time); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const FpaTextPayload& m) +{ + j = nlohmann::json::object({ + { "level", m.level }, + { "text", m.text }, + }); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const FpaTfPayload& m) +{ + j = nlohmann::json::object({ + { "frame_a", m.frame_a }, + { "frame_b", m.frame_b }, + { "translation", m.translation }, + { "orientation", m.orientation }, + }); + j.update(m.gps_time); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const FpaTpPayload& m) +{ + j = nlohmann::json::object({ + { "tp_name", m.tp_name }, + { "timebase", m.timebase }, + { "timeref", m.timeref }, + { "tp_week", m.tp_week }, + { "tp_tow_sec", m.tp_tow_sec }, + { "tp_tow_psec", m.tp_tow_psec }, + { "gps_leaps", m.gps_leaps }, + }); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const FpaVersionPayload& m) +{ + j = nlohmann::json::object({ + { "sw_version", m.sw_version }, + { "hw_name", m.hw_name }, + { "hw_ver", m.hw_ver }, + { "hw_uid", m.hw_uid }, + { "product_model", m.product_model }, + }); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const FpaPayloadPtr& fpa) +{ + j = nlohmann::json::object(); + if (!fpa) { + return; + } + j["_valid"] = fpa->valid_; + if (fpa) { + switch (fpa->msg_type_) { // clang-format off + case FpaMessageType::UNSPECIFIED: break; + case FpaMessageType::EOE: j.update(dynamic_cast(*fpa)); break; + case FpaMessageType::GNSSANT: j.update(dynamic_cast(*fpa)); break; + case FpaMessageType::GNSSCORR: j.update(dynamic_cast(*fpa)); break; + case FpaMessageType::RAWIMU: j.update(dynamic_cast(*fpa)); break; + case FpaMessageType::CORRIMU: j.update(dynamic_cast(*fpa)); break; + case FpaMessageType::IMUBIAS: j.update(dynamic_cast(*fpa)); break; + case FpaMessageType::LLH: j.update(dynamic_cast(*fpa)); break; + case FpaMessageType::ODOMETRY: j.update(dynamic_cast(*fpa)); break; + case FpaMessageType::ODOMENU: j.update(dynamic_cast(*fpa)); break; + case FpaMessageType::ODOMSH: j.update(dynamic_cast(*fpa)); break; + case FpaMessageType::ODOMSTATUS: j.update(dynamic_cast(*fpa)); break; + case FpaMessageType::TEXT: j.update(dynamic_cast(*fpa)); break; + case FpaMessageType::TF: j.update(dynamic_cast(*fpa)); break; + case FpaMessageType::TP: j.update(dynamic_cast(*fpa)); break; + case FpaMessageType::VERSION: j.update(dynamic_cast(*fpa)); break; + } // clang-format on + } +} + +} // namespace fpsdk::common::parser::fpa +/* ****************************************************************************************************************** */ +#endif // !_DOXYGEN_ +#endif // __FPSDK_COMMON_TO_JSON_PARSER_FPA_HPP__ diff --git a/fpsdk_common/include/fpsdk_common/to_json/parser_fpb.hpp b/fpsdk_common/include/fpsdk_common/to_json/parser_fpb.hpp new file mode 100644 index 00000000..3c0e9547 --- /dev/null +++ b/fpsdk_common/include/fpsdk_common/to_json/parser_fpb.hpp @@ -0,0 +1,118 @@ +/** + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ + * \endverbatim + * + * @file + * @brief Fixposition SDK: to_json() helpers for some fpsdk::common::parser::fpb messages + */ +#ifndef __FPSDK_COMMON_TO_JSON_PARSER_FPB_HPP__ +#define __FPSDK_COMMON_TO_JSON_PARSER_FPB_HPP__ + +/* LIBC/STL */ +#include + +/* EXTERNAL */ +#include + +/* PACKAGE */ +#include "../parser/fpb.hpp" +#include "../string.hpp" +#include "../types.hpp" + +#ifndef _DOXYGEN_ // not documenting these +/* ****************************************************************************************************************** */ +namespace fpsdk::common::parser::fpb { + +inline void to_json(nlohmann::json& j, const FpbMeasurementsMeasType e) +{ + switch (e) { // clang-format off + case FpbMeasurementsMeasType::UNSPECIFIED: j = "UNSPECIFIED"; return; + case FpbMeasurementsMeasType::VELOCITY: j = "VELOCITY"; return; + } // clang-format on + j = fpsdk::common::string::Sprintf("BAD_%d", fpsdk::common::types::EnumToVal(e)); +} + +inline void to_json(nlohmann::json& j, const FpbMeasurementsMeasLoc e) +{ + switch (e) { // clang-format off + case FpbMeasurementsMeasLoc::UNSPECIFIED: j = "UNSPECIFIED"; return; + case FpbMeasurementsMeasLoc::RC: j = "RC"; return; + case FpbMeasurementsMeasLoc::FR: j = "FR"; return; + case FpbMeasurementsMeasLoc::FL: j = "FL"; return; + case FpbMeasurementsMeasLoc::RR: j = "RR"; return; + case FpbMeasurementsMeasLoc::RL: j = "RL"; return; + } // clang-format on + j = fpsdk::common::string::Sprintf("BAD_%d", fpsdk::common::types::EnumToVal(e)); +} + +inline void to_json(nlohmann::json& j, const FpbMeasurementsTimestampType e) +{ + switch (e) { // clang-format off + case FpbMeasurementsTimestampType::UNSPECIFIED: j = "UNSPECIFIED"; return; + case FpbMeasurementsTimestampType::TIMEOFARRIVAL: j = "TIMEOFARRIVAL"; return; + case FpbMeasurementsTimestampType::MONOTONIC: j = "MONOTONIC"; return; + case FpbMeasurementsTimestampType::GPS: j = "GPS"; return; + } // clang-format on + j = fpsdk::common::string::Sprintf("BAD_%d", fpsdk::common::types::EnumToVal(e)); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json_FP_B_MEASUREMENTS(nlohmann::json& j, const std::vector& data) +{ + if (data.size() < (FP_B_FRAME_SIZE + FP_B_MEASUREMENTS_HEAD_SIZE)) { + return; + } + FpbMeasurementsHead head; + std::memcpy(&head, &data[FP_B_HEAD_SIZE], sizeof(head)); + if (data.size() != + (FP_B_FRAME_SIZE + FP_B_MEASUREMENTS_HEAD_SIZE + (head.num_meas * FP_B_MEASUREMENTS_MEAS_SIZE))) { + return; + } + j["num_meas"] = head.num_meas; + j["meas"] = nlohmann::json::array(); + for (std::size_t ix = 0; ix < head.num_meas; ix++) { + FpbMeasurementsMeas meas; + std::memcpy(&meas, &data[FP_B_HEAD_SIZE + FP_B_MEASUREMENTS_HEAD_SIZE + (ix * FP_B_MEASUREMENTS_MEAS_SIZE)], + sizeof(meas)); + auto m = nlohmann::json::object(); + auto xyz = nlohmann::json::array({ nullptr, nullptr, nullptr }); + if (meas.meas_x_valid != 0) { + xyz[0] = meas.meas_x; + } + if (meas.meas_y_valid != 0) { + xyz[1] = meas.meas_y; + } + if (meas.meas_z_valid != 0) { + xyz[2] = meas.meas_z; + } + m["xyz"] = xyz; + m["type"] = (FpbMeasurementsMeasType)meas.meas_type; + m["loc"] = (FpbMeasurementsMeasLoc)meas.meas_loc; + m["timestamp"] = (FpbMeasurementsTimestampType)meas.timestamp_type; + m["gps_wno"] = meas.gps_wno; + m["gps_tow"] = meas.gps_tow; + j["meas"].push_back(m); + } +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline nlohmann::json to_json_FP_B(const ParserMsg& msg) +{ + auto j = nlohmann::json::object(); + switch (FpbMsgId(msg.Data())) { // clang-format off + case FP_B_MEASUREMENTS_MSGID: to_json_FP_B_MEASUREMENTS(j, msg.data_); break; + } // clang-format on + return j; +} + +} // namespace fpsdk::common::parser::fpb +/* ****************************************************************************************************************** */ +#endif // !_DOXYGEN_ +#endif // __FPSDK_COMMON_TO_JSON_PARSER_FPB_HPP__ diff --git a/fpsdk_common/include/fpsdk_common/to_json/ros1.hpp b/fpsdk_common/include/fpsdk_common/to_json/ros1.hpp new file mode 100644 index 00000000..bf853604 --- /dev/null +++ b/fpsdk_common/include/fpsdk_common/to_json/ros1.hpp @@ -0,0 +1,198 @@ +/** + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ + * \endverbatim + * + * @file + * @brief Fixposition SDK: to_json() helpers for some ROS1 types + */ +#ifndef __FPSDK_COMMON_TO_JSON_ROS_HPP__ +#define __FPSDK_COMMON_TO_JSON_ROS_HPP__ + +/* LIBC/STL */ + +/* EXTERNAL */ +#include + +/* PACKAGE */ +#include "../ros1.hpp" +#include "../string.hpp" +#include "../time.hpp" + +#ifndef _DOXYGEN_ // not documenting these +/* ****************************************************************************************************************** */ +namespace ros { + +inline void to_json(nlohmann::json& j, const Time& m) +{ + j = nlohmann::json::object({ { "sec", m.sec }, { "nsec", m.nsec } }); +} + +} // namespace ros +/* ****************************************************************************************************************** */ +namespace std_msgs { + +inline void to_json(nlohmann::json& j, const Header& m) +{ + j = nlohmann::json::object({ { "seq", m.seq }, { "stamp", m.stamp }, { "frame_id", m.frame_id } }); +} + +} // namespace std_msgs +/* ****************************************************************************************************************** */ +namespace geometry_msgs { + +inline void to_json(nlohmann::json& j, const Vector3& m) +{ + j = nlohmann::json::object({ { "x", m.x }, { "y", m.y }, { "z", m.z } }); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const Point& m) +{ + j = nlohmann::json::object({ { "x", m.x }, { "y", m.y }, { "z", m.z } }); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const Quaternion& m) +{ + j = nlohmann::json::object({ { "x", m.x }, { "y", m.y }, { "z", m.z }, { "w", m.w } }); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const Transform& m) +{ + j = nlohmann::json::object({ + { "translation", m.translation }, + { "rotation", m.rotation }, + }); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const TransformStamped& m) +{ + j = nlohmann::json::object({ + { "header", m.header }, + { "child_frame_id", m.child_frame_id }, + { "transform", m.transform }, + }); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const Pose& m) +{ + j = nlohmann::json::object({ + { "position", m.position }, + { "orientation", m.orientation }, + }); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const PoseWithCovariance& m) +{ + j = nlohmann::json::object({ + { "pose", m.pose }, + { "covariance", m.covariance }, + }); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const Twist& m) +{ + j = nlohmann::json::object({ + { "linear", m.linear }, + { "angular", m.angular }, + }); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const TwistWithCovariance& m) +{ + j = nlohmann::json::object({ + { "twist", m.twist }, + { "covariance", m.covariance }, + }); +} + +} // namespace geometry_msgs +/* ****************************************************************************************************************** */ +namespace sensor_msgs { + +inline void to_json(nlohmann::json& j, const Imu& m) +{ + j = nlohmann::json::object({ + { "header", m.header }, + { "orientation", m.orientation }, + { "orientation_covariance", m.orientation_covariance }, + { "angular_velocity", m.angular_velocity }, + { "linear_acceleration", m.linear_acceleration }, + { "linear_acceleration_covariance", m.linear_acceleration_covariance }, + }); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const Temperature& m) +{ + j = nlohmann::json::object({ + { "header", m.header }, + { "temperature", m.temperature }, + { "variance", m.variance }, + }); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void to_json(nlohmann::json& j, const Image& m) +{ + j = nlohmann::json::object({ + { "header", m.header }, + { "height", m.height }, + { "width", m.width }, + { "encoding", m.encoding }, + { "is_bigendian", m.is_bigendian }, + { "step", m.step }, + { "data_b64", fpsdk::common::string::Base64Enc(m.data) }, + }); +} + +} // namespace sensor_msgs +/* ****************************************************************************************************************** */ +namespace tf2_msgs { + +inline void to_json(nlohmann::json& j, const TFMessage& m) +{ + j = nlohmann::json::object({ + { "transforms", m.transforms }, + }); +} + +} // namespace tf2_msgs +/* ****************************************************************************************************************** */ +namespace nav_msgs { + +inline void to_json(nlohmann::json& j, const Odometry& m) +{ + j = nlohmann::json::object({ + { "header", m.header }, + { "child_frame_id", m.child_frame_id }, + { "pose", m.pose }, + { "twist", m.twist }, + }); +} + +} // namespace nav_msgs +/* ****************************************************************************************************************** */ +#endif // !_DOXYGEN_ +#endif // __FPSDK_COMMON_TO_JSON_ROS_HPP__ diff --git a/fpsdk_common/include/fpsdk_common/to_json/time.hpp b/fpsdk_common/include/fpsdk_common/to_json/time.hpp new file mode 100644 index 00000000..90da6131 --- /dev/null +++ b/fpsdk_common/include/fpsdk_common/to_json/time.hpp @@ -0,0 +1,36 @@ +/** + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ + * \endverbatim + * + * @file + * @brief Fixposition SDK: to_json() helpers for some fpsdk::common::time types + */ +#ifndef __FPSDK_COMMON_TO_JSON_TIME_HPP__ +#define __FPSDK_COMMON_TO_JSON_TIME_HPP__ + +/* LIBC/STL */ + +/* EXTERNAL */ +#include + +/* PACKAGE */ +#include "../time.hpp" + +#ifndef _DOXYGEN_ // not documenting these +/* ****************************************************************************************************************** */ +namespace fpsdk::common::time { + +inline void to_json(nlohmann::json& j, const RosTime& t) +{ + j = nlohmann::json::array({ t.sec_, t.nsec_ }); +} + +} // namespace fpsdk::common::time +/* ****************************************************************************************************************** */ +#endif // !_DOXYGEN_ +#endif // __FPSDK_COMMON_TO_JSON_TIME_HPP__ diff --git a/fpsdk_common/rosnoros/.clang-format b/fpsdk_common/rosnoros/.clang-format new file mode 100644 index 00000000..e3845288 --- /dev/null +++ b/fpsdk_common/rosnoros/.clang-format @@ -0,0 +1 @@ +DisableFormat: true diff --git a/fpsdk_common/rosnoros/geometry_msgs/Point.h b/fpsdk_common/rosnoros/geometry_msgs/Point.h new file mode 100644 index 00000000..23d9c545 --- /dev/null +++ b/fpsdk_common/rosnoros/geometry_msgs/Point.h @@ -0,0 +1,216 @@ +// Generated by gencpp from file geometry_msgs/Point.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_POINT_H +#define GEOMETRY_MSGS_MESSAGE_POINT_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace geometry_msgs +{ +template +struct Point_ +{ + typedef Point_ Type; + + Point_() + : x(0.0) + , y(0.0) + , z(0.0) { + } + Point_(const ContainerAllocator& _alloc) + : x(0.0) + , y(0.0) + , z(0.0) { + (void)_alloc; + } + + + + typedef double _x_type; + _x_type x; + + typedef double _y_type; + _y_type y; + + typedef double _z_type; + _z_type z; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::Point_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::Point_ const> ConstPtr; + +}; // struct Point_ + +typedef ::geometry_msgs::Point_ > Point; + +typedef boost::shared_ptr< ::geometry_msgs::Point > PointPtr; +typedef boost::shared_ptr< ::geometry_msgs::Point const> PointConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::Point_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::Point_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::geometry_msgs::Point_ & lhs, const ::geometry_msgs::Point_ & rhs) +{ + return lhs.x == rhs.x && + lhs.y == rhs.y && + lhs.z == rhs.z; +} + +template +bool operator!=(const ::geometry_msgs::Point_ & lhs, const ::geometry_msgs::Point_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::geometry_msgs::Point_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Point_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::Point_ > + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::Point_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::Point_ > + : FalseType + { }; + +template +struct HasHeader< ::geometry_msgs::Point_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::geometry_msgs::Point_ > +{ + static const char* value() + { + return "4a842b65f413084dc2b10fb484ea7f17"; + } + + static const char* value(const ::geometry_msgs::Point_&) { return value(); } + static const uint64_t static_value1 = 0x4a842b65f413084dULL; + static const uint64_t static_value2 = 0xc2b10fb484ea7f17ULL; +}; + +template +struct DataType< ::geometry_msgs::Point_ > +{ + static const char* value() + { + return "geometry_msgs/Point"; + } + + static const char* value(const ::geometry_msgs::Point_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::Point_ > +{ + static const char* value() + { + return "# This contains the position of a point in free space\n" +"float64 x\n" +"float64 y\n" +"float64 z\n" +; + } + + static const char* value(const ::geometry_msgs::Point_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::Point_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.x); + stream.next(m.y); + stream.next(m.z); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Point_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::Point_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::Point_& v) + { + s << indent << "x: "; + Printer::stream(s, indent + " ", v.x); + s << indent << "y: "; + Printer::stream(s, indent + " ", v.y); + s << indent << "z: "; + Printer::stream(s, indent + " ", v.z); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_POINT_H diff --git a/fpsdk_common/rosnoros/geometry_msgs/Pose.h b/fpsdk_common/rosnoros/geometry_msgs/Pose.h new file mode 100644 index 00000000..ffa68b4b --- /dev/null +++ b/fpsdk_common/rosnoros/geometry_msgs/Pose.h @@ -0,0 +1,226 @@ +// Generated by gencpp from file geometry_msgs/Pose.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_POSE_H +#define GEOMETRY_MSGS_MESSAGE_POSE_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace geometry_msgs +{ +template +struct Pose_ +{ + typedef Pose_ Type; + + Pose_() + : position() + , orientation() { + } + Pose_(const ContainerAllocator& _alloc) + : position(_alloc) + , orientation(_alloc) { + (void)_alloc; + } + + + + typedef ::geometry_msgs::Point_ _position_type; + _position_type position; + + typedef ::geometry_msgs::Quaternion_ _orientation_type; + _orientation_type orientation; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::Pose_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::Pose_ const> ConstPtr; + +}; // struct Pose_ + +typedef ::geometry_msgs::Pose_ > Pose; + +typedef boost::shared_ptr< ::geometry_msgs::Pose > PosePtr; +typedef boost::shared_ptr< ::geometry_msgs::Pose const> PoseConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::Pose_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::Pose_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::geometry_msgs::Pose_ & lhs, const ::geometry_msgs::Pose_ & rhs) +{ + return lhs.position == rhs.position && + lhs.orientation == rhs.orientation; +} + +template +bool operator!=(const ::geometry_msgs::Pose_ & lhs, const ::geometry_msgs::Pose_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::geometry_msgs::Pose_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Pose_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::Pose_ > + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::Pose_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::Pose_ > + : FalseType + { }; + +template +struct HasHeader< ::geometry_msgs::Pose_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::geometry_msgs::Pose_ > +{ + static const char* value() + { + return "e45d45a5a1ce597b249e23fb30fc871f"; + } + + static const char* value(const ::geometry_msgs::Pose_&) { return value(); } + static const uint64_t static_value1 = 0xe45d45a5a1ce597bULL; + static const uint64_t static_value2 = 0x249e23fb30fc871fULL; +}; + +template +struct DataType< ::geometry_msgs::Pose_ > +{ + static const char* value() + { + return "geometry_msgs/Pose"; + } + + static const char* value(const ::geometry_msgs::Pose_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::Pose_ > +{ + static const char* value() + { + return "# A representation of pose in free space, composed of position and orientation. \n" +"Point position\n" +"Quaternion orientation\n" +"\n" +"================================================================================\n" +"MSG: geometry_msgs/Point\n" +"# This contains the position of a point in free space\n" +"float64 x\n" +"float64 y\n" +"float64 z\n" +"\n" +"================================================================================\n" +"MSG: geometry_msgs/Quaternion\n" +"# This represents an orientation in free space in quaternion form.\n" +"\n" +"float64 x\n" +"float64 y\n" +"float64 z\n" +"float64 w\n" +; + } + + static const char* value(const ::geometry_msgs::Pose_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::Pose_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.position); + stream.next(m.orientation); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Pose_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::Pose_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::Pose_& v) + { + s << indent << "position: "; + s << std::endl; + Printer< ::geometry_msgs::Point_ >::stream(s, indent + " ", v.position); + s << indent << "orientation: "; + s << std::endl; + Printer< ::geometry_msgs::Quaternion_ >::stream(s, indent + " ", v.orientation); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_POSE_H diff --git a/fpsdk_common/rosnoros/geometry_msgs/PoseWithCovariance.h b/fpsdk_common/rosnoros/geometry_msgs/PoseWithCovariance.h new file mode 100644 index 00000000..f8480f1a --- /dev/null +++ b/fpsdk_common/rosnoros/geometry_msgs/PoseWithCovariance.h @@ -0,0 +1,242 @@ +// Generated by gencpp from file geometry_msgs/PoseWithCovariance.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_POSEWITHCOVARIANCE_H +#define GEOMETRY_MSGS_MESSAGE_POSEWITHCOVARIANCE_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace geometry_msgs +{ +template +struct PoseWithCovariance_ +{ + typedef PoseWithCovariance_ Type; + + PoseWithCovariance_() + : pose() + , covariance() { + covariance.assign(0.0); + } + PoseWithCovariance_(const ContainerAllocator& _alloc) + : pose(_alloc) + , covariance() { + (void)_alloc; + covariance.assign(0.0); + } + + + + typedef ::geometry_msgs::Pose_ _pose_type; + _pose_type pose; + + typedef boost::array _covariance_type; + _covariance_type covariance; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::PoseWithCovariance_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::PoseWithCovariance_ const> ConstPtr; + +}; // struct PoseWithCovariance_ + +typedef ::geometry_msgs::PoseWithCovariance_ > PoseWithCovariance; + +typedef boost::shared_ptr< ::geometry_msgs::PoseWithCovariance > PoseWithCovariancePtr; +typedef boost::shared_ptr< ::geometry_msgs::PoseWithCovariance const> PoseWithCovarianceConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::PoseWithCovariance_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::PoseWithCovariance_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::geometry_msgs::PoseWithCovariance_ & lhs, const ::geometry_msgs::PoseWithCovariance_ & rhs) +{ + return lhs.pose == rhs.pose && + lhs.covariance == rhs.covariance; +} + +template +bool operator!=(const ::geometry_msgs::PoseWithCovariance_ & lhs, const ::geometry_msgs::PoseWithCovariance_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::geometry_msgs::PoseWithCovariance_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::PoseWithCovariance_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::PoseWithCovariance_ > + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::PoseWithCovariance_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::PoseWithCovariance_ > + : FalseType + { }; + +template +struct HasHeader< ::geometry_msgs::PoseWithCovariance_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::geometry_msgs::PoseWithCovariance_ > +{ + static const char* value() + { + return "c23e848cf1b7533a8d7c259073a97e6f"; + } + + static const char* value(const ::geometry_msgs::PoseWithCovariance_&) { return value(); } + static const uint64_t static_value1 = 0xc23e848cf1b7533aULL; + static const uint64_t static_value2 = 0x8d7c259073a97e6fULL; +}; + +template +struct DataType< ::geometry_msgs::PoseWithCovariance_ > +{ + static const char* value() + { + return "geometry_msgs/PoseWithCovariance"; + } + + static const char* value(const ::geometry_msgs::PoseWithCovariance_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::PoseWithCovariance_ > +{ + static const char* value() + { + return "# This represents a pose in free space with uncertainty.\n" +"\n" +"Pose pose\n" +"\n" +"# Row-major representation of the 6x6 covariance matrix\n" +"# The orientation parameters use a fixed-axis representation.\n" +"# In order, the parameters are:\n" +"# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)\n" +"float64[36] covariance\n" +"\n" +"================================================================================\n" +"MSG: geometry_msgs/Pose\n" +"# A representation of pose in free space, composed of position and orientation. \n" +"Point position\n" +"Quaternion orientation\n" +"\n" +"================================================================================\n" +"MSG: geometry_msgs/Point\n" +"# This contains the position of a point in free space\n" +"float64 x\n" +"float64 y\n" +"float64 z\n" +"\n" +"================================================================================\n" +"MSG: geometry_msgs/Quaternion\n" +"# This represents an orientation in free space in quaternion form.\n" +"\n" +"float64 x\n" +"float64 y\n" +"float64 z\n" +"float64 w\n" +; + } + + static const char* value(const ::geometry_msgs::PoseWithCovariance_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::PoseWithCovariance_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.pose); + stream.next(m.covariance); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct PoseWithCovariance_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::PoseWithCovariance_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::PoseWithCovariance_& v) + { + s << indent << "pose: "; + s << std::endl; + Printer< ::geometry_msgs::Pose_ >::stream(s, indent + " ", v.pose); + s << indent << "covariance[]" << std::endl; + for (size_t i = 0; i < v.covariance.size(); ++i) + { + s << indent << " covariance[" << i << "]: "; + Printer::stream(s, indent + " ", v.covariance[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_POSEWITHCOVARIANCE_H diff --git a/fpsdk_common/rosnoros/geometry_msgs/Quaternion.h b/fpsdk_common/rosnoros/geometry_msgs/Quaternion.h new file mode 100644 index 00000000..816d47d9 --- /dev/null +++ b/fpsdk_common/rosnoros/geometry_msgs/Quaternion.h @@ -0,0 +1,227 @@ +// Generated by gencpp from file geometry_msgs/Quaternion.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_QUATERNION_H +#define GEOMETRY_MSGS_MESSAGE_QUATERNION_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace geometry_msgs +{ +template +struct Quaternion_ +{ + typedef Quaternion_ Type; + + Quaternion_() + : x(0.0) + , y(0.0) + , z(0.0) + , w(0.0) { + } + Quaternion_(const ContainerAllocator& _alloc) + : x(0.0) + , y(0.0) + , z(0.0) + , w(0.0) { + (void)_alloc; + } + + + + typedef double _x_type; + _x_type x; + + typedef double _y_type; + _y_type y; + + typedef double _z_type; + _z_type z; + + typedef double _w_type; + _w_type w; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::Quaternion_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::Quaternion_ const> ConstPtr; + +}; // struct Quaternion_ + +typedef ::geometry_msgs::Quaternion_ > Quaternion; + +typedef boost::shared_ptr< ::geometry_msgs::Quaternion > QuaternionPtr; +typedef boost::shared_ptr< ::geometry_msgs::Quaternion const> QuaternionConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::Quaternion_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::Quaternion_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::geometry_msgs::Quaternion_ & lhs, const ::geometry_msgs::Quaternion_ & rhs) +{ + return lhs.x == rhs.x && + lhs.y == rhs.y && + lhs.z == rhs.z && + lhs.w == rhs.w; +} + +template +bool operator!=(const ::geometry_msgs::Quaternion_ & lhs, const ::geometry_msgs::Quaternion_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::geometry_msgs::Quaternion_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Quaternion_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::Quaternion_ > + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::Quaternion_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::Quaternion_ > + : FalseType + { }; + +template +struct HasHeader< ::geometry_msgs::Quaternion_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::geometry_msgs::Quaternion_ > +{ + static const char* value() + { + return "a779879fadf0160734f906b8c19c7004"; + } + + static const char* value(const ::geometry_msgs::Quaternion_&) { return value(); } + static const uint64_t static_value1 = 0xa779879fadf01607ULL; + static const uint64_t static_value2 = 0x34f906b8c19c7004ULL; +}; + +template +struct DataType< ::geometry_msgs::Quaternion_ > +{ + static const char* value() + { + return "geometry_msgs/Quaternion"; + } + + static const char* value(const ::geometry_msgs::Quaternion_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::Quaternion_ > +{ + static const char* value() + { + return "# This represents an orientation in free space in quaternion form.\n" +"\n" +"float64 x\n" +"float64 y\n" +"float64 z\n" +"float64 w\n" +; + } + + static const char* value(const ::geometry_msgs::Quaternion_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::Quaternion_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.x); + stream.next(m.y); + stream.next(m.z); + stream.next(m.w); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Quaternion_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::Quaternion_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::Quaternion_& v) + { + s << indent << "x: "; + Printer::stream(s, indent + " ", v.x); + s << indent << "y: "; + Printer::stream(s, indent + " ", v.y); + s << indent << "z: "; + Printer::stream(s, indent + " ", v.z); + s << indent << "w: "; + Printer::stream(s, indent + " ", v.w); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_QUATERNION_H diff --git a/fpsdk_common/rosnoros/geometry_msgs/Transform.h b/fpsdk_common/rosnoros/geometry_msgs/Transform.h new file mode 100644 index 00000000..06fcee0e --- /dev/null +++ b/fpsdk_common/rosnoros/geometry_msgs/Transform.h @@ -0,0 +1,232 @@ +// Generated by gencpp from file geometry_msgs/Transform.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_TRANSFORM_H +#define GEOMETRY_MSGS_MESSAGE_TRANSFORM_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace geometry_msgs +{ +template +struct Transform_ +{ + typedef Transform_ Type; + + Transform_() + : translation() + , rotation() { + } + Transform_(const ContainerAllocator& _alloc) + : translation(_alloc) + , rotation(_alloc) { + (void)_alloc; + } + + + + typedef ::geometry_msgs::Vector3_ _translation_type; + _translation_type translation; + + typedef ::geometry_msgs::Quaternion_ _rotation_type; + _rotation_type rotation; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::Transform_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::Transform_ const> ConstPtr; + +}; // struct Transform_ + +typedef ::geometry_msgs::Transform_ > Transform; + +typedef boost::shared_ptr< ::geometry_msgs::Transform > TransformPtr; +typedef boost::shared_ptr< ::geometry_msgs::Transform const> TransformConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::Transform_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::Transform_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::geometry_msgs::Transform_ & lhs, const ::geometry_msgs::Transform_ & rhs) +{ + return lhs.translation == rhs.translation && + lhs.rotation == rhs.rotation; +} + +template +bool operator!=(const ::geometry_msgs::Transform_ & lhs, const ::geometry_msgs::Transform_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::geometry_msgs::Transform_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Transform_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::Transform_ > + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::Transform_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::Transform_ > + : FalseType + { }; + +template +struct HasHeader< ::geometry_msgs::Transform_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::geometry_msgs::Transform_ > +{ + static const char* value() + { + return "ac9eff44abf714214112b05d54a3cf9b"; + } + + static const char* value(const ::geometry_msgs::Transform_&) { return value(); } + static const uint64_t static_value1 = 0xac9eff44abf71421ULL; + static const uint64_t static_value2 = 0x4112b05d54a3cf9bULL; +}; + +template +struct DataType< ::geometry_msgs::Transform_ > +{ + static const char* value() + { + return "geometry_msgs/Transform"; + } + + static const char* value(const ::geometry_msgs::Transform_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::Transform_ > +{ + static const char* value() + { + return "# This represents the transform between two coordinate frames in free space.\n" +"\n" +"Vector3 translation\n" +"Quaternion rotation\n" +"\n" +"================================================================================\n" +"MSG: geometry_msgs/Vector3\n" +"# This represents a vector in free space. \n" +"# It is only meant to represent a direction. Therefore, it does not\n" +"# make sense to apply a translation to it (e.g., when applying a \n" +"# generic rigid transformation to a Vector3, tf2 will only apply the\n" +"# rotation). If you want your data to be translatable too, use the\n" +"# geometry_msgs/Point message instead.\n" +"\n" +"float64 x\n" +"float64 y\n" +"float64 z\n" +"================================================================================\n" +"MSG: geometry_msgs/Quaternion\n" +"# This represents an orientation in free space in quaternion form.\n" +"\n" +"float64 x\n" +"float64 y\n" +"float64 z\n" +"float64 w\n" +; + } + + static const char* value(const ::geometry_msgs::Transform_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::Transform_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.translation); + stream.next(m.rotation); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Transform_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::Transform_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::Transform_& v) + { + s << indent << "translation: "; + s << std::endl; + Printer< ::geometry_msgs::Vector3_ >::stream(s, indent + " ", v.translation); + s << indent << "rotation: "; + s << std::endl; + Printer< ::geometry_msgs::Quaternion_ >::stream(s, indent + " ", v.rotation); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_TRANSFORM_H diff --git a/fpsdk_common/rosnoros/geometry_msgs/TransformStamped.h b/fpsdk_common/rosnoros/geometry_msgs/TransformStamped.h new file mode 100644 index 00000000..e60938ce --- /dev/null +++ b/fpsdk_common/rosnoros/geometry_msgs/TransformStamped.h @@ -0,0 +1,270 @@ +// Generated by gencpp from file geometry_msgs/TransformStamped.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_TRANSFORMSTAMPED_H +#define GEOMETRY_MSGS_MESSAGE_TRANSFORMSTAMPED_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace geometry_msgs +{ +template +struct TransformStamped_ +{ + typedef TransformStamped_ Type; + + TransformStamped_() + : header() + , child_frame_id() + , transform() { + } + TransformStamped_(const ContainerAllocator& _alloc) + : header(_alloc) + , child_frame_id(_alloc) + , transform(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef std::basic_string, typename std::allocator_traits::template rebind_alloc> _child_frame_id_type; + _child_frame_id_type child_frame_id; + + typedef ::geometry_msgs::Transform_ _transform_type; + _transform_type transform; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::TransformStamped_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::TransformStamped_ const> ConstPtr; + +}; // struct TransformStamped_ + +typedef ::geometry_msgs::TransformStamped_ > TransformStamped; + +typedef boost::shared_ptr< ::geometry_msgs::TransformStamped > TransformStampedPtr; +typedef boost::shared_ptr< ::geometry_msgs::TransformStamped const> TransformStampedConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::TransformStamped_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::TransformStamped_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::geometry_msgs::TransformStamped_ & lhs, const ::geometry_msgs::TransformStamped_ & rhs) +{ + return lhs.header == rhs.header && + lhs.child_frame_id == rhs.child_frame_id && + lhs.transform == rhs.transform; +} + +template +bool operator!=(const ::geometry_msgs::TransformStamped_ & lhs, const ::geometry_msgs::TransformStamped_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::geometry_msgs::TransformStamped_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::TransformStamped_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::TransformStamped_ > + : FalseType + { }; + +template +struct IsFixedSize< ::geometry_msgs::TransformStamped_ const> + : FalseType + { }; + +template +struct HasHeader< ::geometry_msgs::TransformStamped_ > + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::TransformStamped_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::geometry_msgs::TransformStamped_ > +{ + static const char* value() + { + return "b5764a33bfeb3588febc2682852579b0"; + } + + static const char* value(const ::geometry_msgs::TransformStamped_&) { return value(); } + static const uint64_t static_value1 = 0xb5764a33bfeb3588ULL; + static const uint64_t static_value2 = 0xfebc2682852579b0ULL; +}; + +template +struct DataType< ::geometry_msgs::TransformStamped_ > +{ + static const char* value() + { + return "geometry_msgs/TransformStamped"; + } + + static const char* value(const ::geometry_msgs::TransformStamped_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::TransformStamped_ > +{ + static const char* value() + { + return "# This expresses a transform from coordinate frame header.frame_id\n" +"# to the coordinate frame child_frame_id\n" +"#\n" +"# This message is mostly used by the \n" +"# tf package. \n" +"# See its documentation for more information.\n" +"\n" +"Header header\n" +"string child_frame_id # the frame id of the child frame\n" +"Transform transform\n" +"\n" +"================================================================================\n" +"MSG: std_msgs/Header\n" +"# Standard metadata for higher-level stamped data types.\n" +"# This is generally used to communicate timestamped data \n" +"# in a particular coordinate frame.\n" +"# \n" +"# sequence ID: consecutively increasing ID \n" +"uint32 seq\n" +"#Two-integer timestamp that is expressed as:\n" +"# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n" +"# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n" +"# time-handling sugar is provided by the client library\n" +"time stamp\n" +"#Frame this data is associated with\n" +"string frame_id\n" +"\n" +"================================================================================\n" +"MSG: geometry_msgs/Transform\n" +"# This represents the transform between two coordinate frames in free space.\n" +"\n" +"Vector3 translation\n" +"Quaternion rotation\n" +"\n" +"================================================================================\n" +"MSG: geometry_msgs/Vector3\n" +"# This represents a vector in free space. \n" +"# It is only meant to represent a direction. Therefore, it does not\n" +"# make sense to apply a translation to it (e.g., when applying a \n" +"# generic rigid transformation to a Vector3, tf2 will only apply the\n" +"# rotation). If you want your data to be translatable too, use the\n" +"# geometry_msgs/Point message instead.\n" +"\n" +"float64 x\n" +"float64 y\n" +"float64 z\n" +"================================================================================\n" +"MSG: geometry_msgs/Quaternion\n" +"# This represents an orientation in free space in quaternion form.\n" +"\n" +"float64 x\n" +"float64 y\n" +"float64 z\n" +"float64 w\n" +; + } + + static const char* value(const ::geometry_msgs::TransformStamped_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::TransformStamped_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.child_frame_id); + stream.next(m.transform); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct TransformStamped_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::TransformStamped_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::TransformStamped_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "child_frame_id: "; + Printer, typename std::allocator_traits::template rebind_alloc>>::stream(s, indent + " ", v.child_frame_id); + s << indent << "transform: "; + s << std::endl; + Printer< ::geometry_msgs::Transform_ >::stream(s, indent + " ", v.transform); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_TRANSFORMSTAMPED_H diff --git a/fpsdk_common/rosnoros/geometry_msgs/Twist.h b/fpsdk_common/rosnoros/geometry_msgs/Twist.h new file mode 100644 index 00000000..992e526b --- /dev/null +++ b/fpsdk_common/rosnoros/geometry_msgs/Twist.h @@ -0,0 +1,223 @@ +// Generated by gencpp from file geometry_msgs/Twist.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_TWIST_H +#define GEOMETRY_MSGS_MESSAGE_TWIST_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +namespace geometry_msgs +{ +template +struct Twist_ +{ + typedef Twist_ Type; + + Twist_() + : linear() + , angular() { + } + Twist_(const ContainerAllocator& _alloc) + : linear(_alloc) + , angular(_alloc) { + (void)_alloc; + } + + + + typedef ::geometry_msgs::Vector3_ _linear_type; + _linear_type linear; + + typedef ::geometry_msgs::Vector3_ _angular_type; + _angular_type angular; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::Twist_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::Twist_ const> ConstPtr; + +}; // struct Twist_ + +typedef ::geometry_msgs::Twist_ > Twist; + +typedef boost::shared_ptr< ::geometry_msgs::Twist > TwistPtr; +typedef boost::shared_ptr< ::geometry_msgs::Twist const> TwistConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::Twist_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::Twist_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::geometry_msgs::Twist_ & lhs, const ::geometry_msgs::Twist_ & rhs) +{ + return lhs.linear == rhs.linear && + lhs.angular == rhs.angular; +} + +template +bool operator!=(const ::geometry_msgs::Twist_ & lhs, const ::geometry_msgs::Twist_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::geometry_msgs::Twist_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Twist_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::Twist_ > + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::Twist_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::Twist_ > + : FalseType + { }; + +template +struct HasHeader< ::geometry_msgs::Twist_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::geometry_msgs::Twist_ > +{ + static const char* value() + { + return "9f195f881246fdfa2798d1d3eebca84a"; + } + + static const char* value(const ::geometry_msgs::Twist_&) { return value(); } + static const uint64_t static_value1 = 0x9f195f881246fdfaULL; + static const uint64_t static_value2 = 0x2798d1d3eebca84aULL; +}; + +template +struct DataType< ::geometry_msgs::Twist_ > +{ + static const char* value() + { + return "geometry_msgs/Twist"; + } + + static const char* value(const ::geometry_msgs::Twist_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::Twist_ > +{ + static const char* value() + { + return "# This expresses velocity in free space broken into its linear and angular parts.\n" +"Vector3 linear\n" +"Vector3 angular\n" +"\n" +"================================================================================\n" +"MSG: geometry_msgs/Vector3\n" +"# This represents a vector in free space. \n" +"# It is only meant to represent a direction. Therefore, it does not\n" +"# make sense to apply a translation to it (e.g., when applying a \n" +"# generic rigid transformation to a Vector3, tf2 will only apply the\n" +"# rotation). If you want your data to be translatable too, use the\n" +"# geometry_msgs/Point message instead.\n" +"\n" +"float64 x\n" +"float64 y\n" +"float64 z\n" +; + } + + static const char* value(const ::geometry_msgs::Twist_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::Twist_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.linear); + stream.next(m.angular); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Twist_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::Twist_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::Twist_& v) + { + s << indent << "linear: "; + s << std::endl; + Printer< ::geometry_msgs::Vector3_ >::stream(s, indent + " ", v.linear); + s << indent << "angular: "; + s << std::endl; + Printer< ::geometry_msgs::Vector3_ >::stream(s, indent + " ", v.angular); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_TWIST_H diff --git a/fpsdk_common/rosnoros/geometry_msgs/TwistWithCovariance.h b/fpsdk_common/rosnoros/geometry_msgs/TwistWithCovariance.h new file mode 100644 index 00000000..012a2a50 --- /dev/null +++ b/fpsdk_common/rosnoros/geometry_msgs/TwistWithCovariance.h @@ -0,0 +1,239 @@ +// Generated by gencpp from file geometry_msgs/TwistWithCovariance.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_TWISTWITHCOVARIANCE_H +#define GEOMETRY_MSGS_MESSAGE_TWISTWITHCOVARIANCE_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace geometry_msgs +{ +template +struct TwistWithCovariance_ +{ + typedef TwistWithCovariance_ Type; + + TwistWithCovariance_() + : twist() + , covariance() { + covariance.assign(0.0); + } + TwistWithCovariance_(const ContainerAllocator& _alloc) + : twist(_alloc) + , covariance() { + (void)_alloc; + covariance.assign(0.0); + } + + + + typedef ::geometry_msgs::Twist_ _twist_type; + _twist_type twist; + + typedef boost::array _covariance_type; + _covariance_type covariance; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::TwistWithCovariance_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::TwistWithCovariance_ const> ConstPtr; + +}; // struct TwistWithCovariance_ + +typedef ::geometry_msgs::TwistWithCovariance_ > TwistWithCovariance; + +typedef boost::shared_ptr< ::geometry_msgs::TwistWithCovariance > TwistWithCovariancePtr; +typedef boost::shared_ptr< ::geometry_msgs::TwistWithCovariance const> TwistWithCovarianceConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::TwistWithCovariance_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::TwistWithCovariance_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::geometry_msgs::TwistWithCovariance_ & lhs, const ::geometry_msgs::TwistWithCovariance_ & rhs) +{ + return lhs.twist == rhs.twist && + lhs.covariance == rhs.covariance; +} + +template +bool operator!=(const ::geometry_msgs::TwistWithCovariance_ & lhs, const ::geometry_msgs::TwistWithCovariance_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::geometry_msgs::TwistWithCovariance_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::TwistWithCovariance_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::TwistWithCovariance_ > + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::TwistWithCovariance_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::TwistWithCovariance_ > + : FalseType + { }; + +template +struct HasHeader< ::geometry_msgs::TwistWithCovariance_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::geometry_msgs::TwistWithCovariance_ > +{ + static const char* value() + { + return "1fe8a28e6890a4cc3ae4c3ca5c7d82e6"; + } + + static const char* value(const ::geometry_msgs::TwistWithCovariance_&) { return value(); } + static const uint64_t static_value1 = 0x1fe8a28e6890a4ccULL; + static const uint64_t static_value2 = 0x3ae4c3ca5c7d82e6ULL; +}; + +template +struct DataType< ::geometry_msgs::TwistWithCovariance_ > +{ + static const char* value() + { + return "geometry_msgs/TwistWithCovariance"; + } + + static const char* value(const ::geometry_msgs::TwistWithCovariance_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::TwistWithCovariance_ > +{ + static const char* value() + { + return "# This expresses velocity in free space with uncertainty.\n" +"\n" +"Twist twist\n" +"\n" +"# Row-major representation of the 6x6 covariance matrix\n" +"# The orientation parameters use a fixed-axis representation.\n" +"# In order, the parameters are:\n" +"# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)\n" +"float64[36] covariance\n" +"\n" +"================================================================================\n" +"MSG: geometry_msgs/Twist\n" +"# This expresses velocity in free space broken into its linear and angular parts.\n" +"Vector3 linear\n" +"Vector3 angular\n" +"\n" +"================================================================================\n" +"MSG: geometry_msgs/Vector3\n" +"# This represents a vector in free space. \n" +"# It is only meant to represent a direction. Therefore, it does not\n" +"# make sense to apply a translation to it (e.g., when applying a \n" +"# generic rigid transformation to a Vector3, tf2 will only apply the\n" +"# rotation). If you want your data to be translatable too, use the\n" +"# geometry_msgs/Point message instead.\n" +"\n" +"float64 x\n" +"float64 y\n" +"float64 z\n" +; + } + + static const char* value(const ::geometry_msgs::TwistWithCovariance_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::TwistWithCovariance_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.twist); + stream.next(m.covariance); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct TwistWithCovariance_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::TwistWithCovariance_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::TwistWithCovariance_& v) + { + s << indent << "twist: "; + s << std::endl; + Printer< ::geometry_msgs::Twist_ >::stream(s, indent + " ", v.twist); + s << indent << "covariance[]" << std::endl; + for (size_t i = 0; i < v.covariance.size(); ++i) + { + s << indent << " covariance[" << i << "]: "; + Printer::stream(s, indent + " ", v.covariance[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_TWISTWITHCOVARIANCE_H diff --git a/fpsdk_common/rosnoros/geometry_msgs/Vector3.h b/fpsdk_common/rosnoros/geometry_msgs/Vector3.h new file mode 100644 index 00000000..6fe3ce30 --- /dev/null +++ b/fpsdk_common/rosnoros/geometry_msgs/Vector3.h @@ -0,0 +1,222 @@ +// Generated by gencpp from file geometry_msgs/Vector3.msg +// DO NOT EDIT! + + +#ifndef GEOMETRY_MSGS_MESSAGE_VECTOR3_H +#define GEOMETRY_MSGS_MESSAGE_VECTOR3_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace geometry_msgs +{ +template +struct Vector3_ +{ + typedef Vector3_ Type; + + Vector3_() + : x(0.0) + , y(0.0) + , z(0.0) { + } + Vector3_(const ContainerAllocator& _alloc) + : x(0.0) + , y(0.0) + , z(0.0) { + (void)_alloc; + } + + + + typedef double _x_type; + _x_type x; + + typedef double _y_type; + _y_type y; + + typedef double _z_type; + _z_type z; + + + + + + typedef boost::shared_ptr< ::geometry_msgs::Vector3_ > Ptr; + typedef boost::shared_ptr< ::geometry_msgs::Vector3_ const> ConstPtr; + +}; // struct Vector3_ + +typedef ::geometry_msgs::Vector3_ > Vector3; + +typedef boost::shared_ptr< ::geometry_msgs::Vector3 > Vector3Ptr; +typedef boost::shared_ptr< ::geometry_msgs::Vector3 const> Vector3ConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::geometry_msgs::Vector3_ & v) +{ +ros::message_operations::Printer< ::geometry_msgs::Vector3_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::geometry_msgs::Vector3_ & lhs, const ::geometry_msgs::Vector3_ & rhs) +{ + return lhs.x == rhs.x && + lhs.y == rhs.y && + lhs.z == rhs.z; +} + +template +bool operator!=(const ::geometry_msgs::Vector3_ & lhs, const ::geometry_msgs::Vector3_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace geometry_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::geometry_msgs::Vector3_ > + : TrueType + { }; + +template +struct IsMessage< ::geometry_msgs::Vector3_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::Vector3_ > + : TrueType + { }; + +template +struct IsFixedSize< ::geometry_msgs::Vector3_ const> + : TrueType + { }; + +template +struct HasHeader< ::geometry_msgs::Vector3_ > + : FalseType + { }; + +template +struct HasHeader< ::geometry_msgs::Vector3_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::geometry_msgs::Vector3_ > +{ + static const char* value() + { + return "4a842b65f413084dc2b10fb484ea7f17"; + } + + static const char* value(const ::geometry_msgs::Vector3_&) { return value(); } + static const uint64_t static_value1 = 0x4a842b65f413084dULL; + static const uint64_t static_value2 = 0xc2b10fb484ea7f17ULL; +}; + +template +struct DataType< ::geometry_msgs::Vector3_ > +{ + static const char* value() + { + return "geometry_msgs/Vector3"; + } + + static const char* value(const ::geometry_msgs::Vector3_&) { return value(); } +}; + +template +struct Definition< ::geometry_msgs::Vector3_ > +{ + static const char* value() + { + return "# This represents a vector in free space. \n" +"# It is only meant to represent a direction. Therefore, it does not\n" +"# make sense to apply a translation to it (e.g., when applying a \n" +"# generic rigid transformation to a Vector3, tf2 will only apply the\n" +"# rotation). If you want your data to be translatable too, use the\n" +"# geometry_msgs/Point message instead.\n" +"\n" +"float64 x\n" +"float64 y\n" +"float64 z\n" +; + } + + static const char* value(const ::geometry_msgs::Vector3_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::geometry_msgs::Vector3_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.x); + stream.next(m.y); + stream.next(m.z); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Vector3_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::geometry_msgs::Vector3_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::geometry_msgs::Vector3_& v) + { + s << indent << "x: "; + Printer::stream(s, indent + " ", v.x); + s << indent << "y: "; + Printer::stream(s, indent + " ", v.y); + s << indent << "z: "; + Printer::stream(s, indent + " ", v.z); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // GEOMETRY_MSGS_MESSAGE_VECTOR3_H diff --git a/fpsdk_common/rosnoros/nav_msgs/Odometry.h b/fpsdk_common/rosnoros/nav_msgs/Odometry.h new file mode 100644 index 00000000..73478901 --- /dev/null +++ b/fpsdk_common/rosnoros/nav_msgs/Odometry.h @@ -0,0 +1,315 @@ +// Generated by gencpp from file nav_msgs/Odometry.msg +// DO NOT EDIT! + + +#ifndef NAV_MSGS_MESSAGE_ODOMETRY_H +#define NAV_MSGS_MESSAGE_ODOMETRY_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace nav_msgs +{ +template +struct Odometry_ +{ + typedef Odometry_ Type; + + Odometry_() + : header() + , child_frame_id() + , pose() + , twist() { + } + Odometry_(const ContainerAllocator& _alloc) + : header(_alloc) + , child_frame_id(_alloc) + , pose(_alloc) + , twist(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef std::basic_string, typename std::allocator_traits::template rebind_alloc> _child_frame_id_type; + _child_frame_id_type child_frame_id; + + typedef ::geometry_msgs::PoseWithCovariance_ _pose_type; + _pose_type pose; + + typedef ::geometry_msgs::TwistWithCovariance_ _twist_type; + _twist_type twist; + + + + + + typedef boost::shared_ptr< ::nav_msgs::Odometry_ > Ptr; + typedef boost::shared_ptr< ::nav_msgs::Odometry_ const> ConstPtr; + +}; // struct Odometry_ + +typedef ::nav_msgs::Odometry_ > Odometry; + +typedef boost::shared_ptr< ::nav_msgs::Odometry > OdometryPtr; +typedef boost::shared_ptr< ::nav_msgs::Odometry const> OdometryConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::nav_msgs::Odometry_ & v) +{ +ros::message_operations::Printer< ::nav_msgs::Odometry_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::nav_msgs::Odometry_ & lhs, const ::nav_msgs::Odometry_ & rhs) +{ + return lhs.header == rhs.header && + lhs.child_frame_id == rhs.child_frame_id && + lhs.pose == rhs.pose && + lhs.twist == rhs.twist; +} + +template +bool operator!=(const ::nav_msgs::Odometry_ & lhs, const ::nav_msgs::Odometry_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace nav_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::nav_msgs::Odometry_ > + : TrueType + { }; + +template +struct IsMessage< ::nav_msgs::Odometry_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::nav_msgs::Odometry_ > + : FalseType + { }; + +template +struct IsFixedSize< ::nav_msgs::Odometry_ const> + : FalseType + { }; + +template +struct HasHeader< ::nav_msgs::Odometry_ > + : TrueType + { }; + +template +struct HasHeader< ::nav_msgs::Odometry_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::nav_msgs::Odometry_ > +{ + static const char* value() + { + return "cd5e73d190d741a2f92e81eda573aca7"; + } + + static const char* value(const ::nav_msgs::Odometry_&) { return value(); } + static const uint64_t static_value1 = 0xcd5e73d190d741a2ULL; + static const uint64_t static_value2 = 0xf92e81eda573aca7ULL; +}; + +template +struct DataType< ::nav_msgs::Odometry_ > +{ + static const char* value() + { + return "nav_msgs/Odometry"; + } + + static const char* value(const ::nav_msgs::Odometry_&) { return value(); } +}; + +template +struct Definition< ::nav_msgs::Odometry_ > +{ + static const char* value() + { + return "# This represents an estimate of a position and velocity in free space. \n" +"# The pose in this message should be specified in the coordinate frame given by header.frame_id.\n" +"# The twist in this message should be specified in the coordinate frame given by the child_frame_id\n" +"Header header\n" +"string child_frame_id\n" +"geometry_msgs/PoseWithCovariance pose\n" +"geometry_msgs/TwistWithCovariance twist\n" +"\n" +"================================================================================\n" +"MSG: std_msgs/Header\n" +"# Standard metadata for higher-level stamped data types.\n" +"# This is generally used to communicate timestamped data \n" +"# in a particular coordinate frame.\n" +"# \n" +"# sequence ID: consecutively increasing ID \n" +"uint32 seq\n" +"#Two-integer timestamp that is expressed as:\n" +"# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n" +"# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n" +"# time-handling sugar is provided by the client library\n" +"time stamp\n" +"#Frame this data is associated with\n" +"string frame_id\n" +"\n" +"================================================================================\n" +"MSG: geometry_msgs/PoseWithCovariance\n" +"# This represents a pose in free space with uncertainty.\n" +"\n" +"Pose pose\n" +"\n" +"# Row-major representation of the 6x6 covariance matrix\n" +"# The orientation parameters use a fixed-axis representation.\n" +"# In order, the parameters are:\n" +"# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)\n" +"float64[36] covariance\n" +"\n" +"================================================================================\n" +"MSG: geometry_msgs/Pose\n" +"# A representation of pose in free space, composed of position and orientation. \n" +"Point position\n" +"Quaternion orientation\n" +"\n" +"================================================================================\n" +"MSG: geometry_msgs/Point\n" +"# This contains the position of a point in free space\n" +"float64 x\n" +"float64 y\n" +"float64 z\n" +"\n" +"================================================================================\n" +"MSG: geometry_msgs/Quaternion\n" +"# This represents an orientation in free space in quaternion form.\n" +"\n" +"float64 x\n" +"float64 y\n" +"float64 z\n" +"float64 w\n" +"\n" +"================================================================================\n" +"MSG: geometry_msgs/TwistWithCovariance\n" +"# This expresses velocity in free space with uncertainty.\n" +"\n" +"Twist twist\n" +"\n" +"# Row-major representation of the 6x6 covariance matrix\n" +"# The orientation parameters use a fixed-axis representation.\n" +"# In order, the parameters are:\n" +"# (x, y, z, rotation about X axis, rotation about Y axis, rotation about Z axis)\n" +"float64[36] covariance\n" +"\n" +"================================================================================\n" +"MSG: geometry_msgs/Twist\n" +"# This expresses velocity in free space broken into its linear and angular parts.\n" +"Vector3 linear\n" +"Vector3 angular\n" +"\n" +"================================================================================\n" +"MSG: geometry_msgs/Vector3\n" +"# This represents a vector in free space. \n" +"# It is only meant to represent a direction. Therefore, it does not\n" +"# make sense to apply a translation to it (e.g., when applying a \n" +"# generic rigid transformation to a Vector3, tf2 will only apply the\n" +"# rotation). If you want your data to be translatable too, use the\n" +"# geometry_msgs/Point message instead.\n" +"\n" +"float64 x\n" +"float64 y\n" +"float64 z\n" +; + } + + static const char* value(const ::nav_msgs::Odometry_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::nav_msgs::Odometry_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.child_frame_id); + stream.next(m.pose); + stream.next(m.twist); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Odometry_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::nav_msgs::Odometry_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::nav_msgs::Odometry_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "child_frame_id: "; + Printer, typename std::allocator_traits::template rebind_alloc>>::stream(s, indent + " ", v.child_frame_id); + s << indent << "pose: "; + s << std::endl; + Printer< ::geometry_msgs::PoseWithCovariance_ >::stream(s, indent + " ", v.pose); + s << indent << "twist: "; + s << std::endl; + Printer< ::geometry_msgs::TwistWithCovariance_ >::stream(s, indent + " ", v.twist); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // NAV_MSGS_MESSAGE_ODOMETRY_H diff --git a/fpsdk_common/rosnoros/ros/builtin_message_traits.h b/fpsdk_common/rosnoros/ros/builtin_message_traits.h new file mode 100644 index 00000000..478bc5fa --- /dev/null +++ b/fpsdk_common/rosnoros/ros/builtin_message_traits.h @@ -0,0 +1,424 @@ +/* + * 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 message_traits polyfill + * + * @details Files impoorted: + * - ros/message_forward.h + * - ros/message_traits.h + * - ros/builtin_message_traits.h + * + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * + */ + +#ifndef __ROSNOROS__BUILTIN_MESSAGE_TRAITS_H__ +#define __ROSNOROS__BUILTIN_MESSAGE_TRAITS_H__ + +/**********************************************************************************************************************/ +// ros/message_forward.h +/**********************************************************************************************************************/ +#include // IWYU pragma: keep + +// C++ standard section 17.4.3.1/1 states that forward declarations of STL types +// that aren't specializations involving user defined types results in undefined +// behavior. Apparently only libc++ has a problem with this and won't compile it. +#ifndef _LIBCPP_VERSION +namespace std { +template +class allocator; +} +#else +#include +#endif + +namespace boost { +template +class shared_ptr; +} + +/** + * \brief Forward-declare a message, including Ptr and ConstPtr types, with an allocator + * + * \param msg The "base" message type, i.e., the name of the .msg file + * \param new_name The name you'd like the message to have + * \param alloc The allocator to use, e.g. std::allocator + */ +#define ROS_DECLARE_MESSAGE_WITH_ALLOCATOR(msg, new_name, alloc) \ + template \ + struct msg##_; \ + typedef msg##_ > new_name; \ + typedef boost::shared_ptr new_name##Ptr; \ + typedef boost::shared_ptr new_name##ConstPtr; + +/** + * \brief Forward-declare a message, including Ptr and ConstPtr types, using std::allocator + * \param msg The "base" message type, i.e. the name of the .msg file + */ +#define ROS_DECLARE_MESSAGE(msg) ROS_DECLARE_MESSAGE_WITH_ALLOCATOR(msg, msg, std::allocator) + +/**********************************************************************************************************************/ +// ros/message_traits.h +/**********************************************************************************************************************/ +#include +#include +#include +#include + +#include "time.h" + +namespace std_msgs { +ROS_DECLARE_MESSAGE(Header) +} + +#define ROS_IMPLEMENT_SIMPLE_TOPIC_TRAITS(msg, md5sum, datatype, definition) \ + namespace ros { \ + namespace message_traits { \ + template <> \ + struct MD5Sum { \ + static const char* value() { return md5sum; } \ + static const char* value(const msg&) { return value(); } \ + }; \ + template <> \ + struct DataType { \ + static const char* value() { return datatype; } \ + static const char* value(const msg&) { return value(); } \ + }; \ + template <> \ + struct Definition { \ + static const char* value() { return definition; } \ + static const char* value(const msg&) { return value(); } \ + }; \ + } \ + } + +namespace ros { +namespace message_traits { + +/** + * \brief Base type for compile-type true/false tests. Compatible with Boost.MPL. classes inheriting from this type + * are \b true values. + */ +struct TrueType { + static const bool value = true; + typedef TrueType type; +}; + +/** + * \brief Base type for compile-type true/false tests. Compatible with Boost.MPL. classes inheriting from this type + * are \b false values. + */ +struct FalseType { + static const bool value = false; + typedef FalseType type; +}; + +/** + * \brief A simple datatype is one that can be memcpy'd directly in array form, i.e. it's a POD, fixed-size type and + * sizeof(M) == sum(serializationLength(M:a...)) + */ +template +struct IsSimple : public FalseType {}; +/** + * \brief A fixed-size datatype is one whose size is constant, i.e. it has no variable-length arrays or strings + */ +template +struct IsFixedSize : public FalseType {}; +/** + * \brief HasHeader informs whether or not there is a header that gets serialized as the first thing in the message + */ +template +struct HasHeader : public FalseType {}; + +/** + * \brief Am I message or not + */ +template +struct IsMessage : public FalseType {}; + +/** + * \brief Specialize to provide the md5sum for a message + */ +template +struct MD5Sum { + static const char* value() { return M::__s_getMD5Sum().c_str(); } + + static const char* value(const M& m) { return m.__getMD5Sum().c_str(); } +}; + +/** + * \brief Specialize to provide the datatype for a message + */ +template +struct DataType { + static const char* value() { return M::__s_getDataType().c_str(); } + + static const char* value(const M& m) { return m.__getDataType().c_str(); } +}; + +/** + * \brief Specialize to provide the definition for a message + */ +template +struct Definition { + static const char* value() { return M::__s_getMessageDefinition().c_str(); } + + static const char* value(const M& m) { return m.__getMessageDefinition().c_str(); } +}; + +/** + * \brief Header trait. In the default implementation pointer() + * returns &m.header if HasHeader::value is true, otherwise returns NULL + */ +template +struct Header { + static std_msgs::Header* pointer(M& m) { + (void)m; + return 0; + } + static std_msgs::Header const* pointer(const M& m) { + (void)m; + return 0; + } +}; + +template +struct Header >::type> { + static std_msgs::Header* pointer(M& m) { return &m.header; } + static std_msgs::Header const* pointer(const M& m) { return &m.header; } +}; + +/** + * \brief FrameId trait. In the default implementation pointer() + * returns &m.header.frame_id if HasHeader::value is true, otherwise returns NULL. value() + * does not exist, and causes a compile error + */ +template +struct FrameId { + static std::string* pointer(M& m) { + (void)m; + return 0; + } + static std::string const* pointer(const M& m) { + (void)m; + return 0; + } +}; + +template +struct FrameId >::type> { + static std::string* pointer(M& m) { return &m.header.frame_id; } + static std::string const* pointer(const M& m) { return &m.header.frame_id; } + static std::string value(const M& m) { return m.header.frame_id; } +}; + +/** + * \brief TimeStamp trait. In the default implementation pointer() + * returns &m.header.stamp if HasHeader::value is true, otherwise returns NULL. value() + * does not exist, and causes a compile error + */ +template +struct TimeStamp { + static ros::Time* pointer(M& m) { + (void)m; + return 0; + } + static ros::Time const* pointer(const M& m) { + (void)m; + return 0; + } +}; + +template +struct TimeStamp >::type> { + static ros::Time* pointer(typename boost::remove_const::type& m) { return &m.header.stamp; } + static ros::Time const* pointer(const M& m) { return &m.header.stamp; } + static ros::Time value(const M& m) { return m.header.stamp; } +}; + +/** + * \brief returns MD5Sum::value(); + */ +template +inline const char* md5sum() { + return MD5Sum::type>::type>::value(); +} + +/** + * \brief returns DataType::value(); + */ +template +inline const char* datatype() { + return DataType::type>::type>::value(); +} + +/** + * \brief returns Definition::value(); + */ +template +inline const char* definition() { + return Definition::type>::type>::value(); +} + +/** + * \brief returns MD5Sum::value(m); + */ +template +inline const char* md5sum(const M& m) { + return MD5Sum::type>::type>::value(m); +} + +/** + * \brief returns DataType::value(m); + */ +template +inline const char* datatype(const M& m) { + return DataType::type>::type>::value(m); +} + +/** + * \brief returns Definition::value(m); + */ +template +inline const char* definition(const M& m) { + return Definition::type>::type>::value(m); +} + +/** + * \brief returns Header::pointer(m); + */ +template +inline std_msgs::Header* header(M& m) { + return Header::type>::type>::pointer(m); +} + +/** + * \brief returns Header::pointer(m); + */ +template +inline std_msgs::Header const* header(const M& m) { + return Header::type>::type>::pointer(m); +} + +/** + * \brief returns FrameId::pointer(m); + */ +template +inline std::string* frameId(M& m) { + return FrameId::type>::type>::pointer(m); +} + +/** + * \brief returns FrameId::pointer(m); + */ +template +inline std::string const* frameId(const M& m) { + return FrameId::type>::type>::pointer(m); +} + +/** + * \brief returns TimeStamp::pointer(m); + */ +template +inline ros::Time* timeStamp(M& m) { + return TimeStamp::type>::type>::pointer(m); +} + +/** + * \brief returns TimeStamp::pointer(m); + */ +template +inline ros::Time const* timeStamp(const M& m) { + return TimeStamp::type>::type>::pointer(m); +} + +/** + * \brief returns IsSimple::value; + */ +template +inline bool isSimple() { + return IsSimple::type>::type>::value; +} + +/** + * \brief returns IsFixedSize::value; + */ +template +inline bool isFixedSize() { + return IsFixedSize::type>::type>::value; +} + +/** + * \brief returns HasHeader::value; + */ +template +inline bool hasHeader() { + return HasHeader::type>::type>::value; +} + +} // namespace message_traits +} // namespace ros + +/**********************************************************************************************************************/ +// ros/builtin_message_traits.h +/**********************************************************************************************************************/ +namespace ros { +namespace message_traits { + +#define ROSLIB_CREATE_SIMPLE_TRAITS(Type) \ + template <> \ + struct IsSimple : public TrueType {}; \ + template <> \ + struct IsFixedSize : public TrueType {}; + +ROSLIB_CREATE_SIMPLE_TRAITS(uint8_t) +ROSLIB_CREATE_SIMPLE_TRAITS(int8_t) +ROSLIB_CREATE_SIMPLE_TRAITS(uint16_t) +ROSLIB_CREATE_SIMPLE_TRAITS(int16_t) +ROSLIB_CREATE_SIMPLE_TRAITS(uint32_t) +ROSLIB_CREATE_SIMPLE_TRAITS(int32_t) +ROSLIB_CREATE_SIMPLE_TRAITS(uint64_t) +ROSLIB_CREATE_SIMPLE_TRAITS(int64_t) +ROSLIB_CREATE_SIMPLE_TRAITS(float) +ROSLIB_CREATE_SIMPLE_TRAITS(double) +ROSLIB_CREATE_SIMPLE_TRAITS(Time) +ROSLIB_CREATE_SIMPLE_TRAITS(Duration) + +// because std::vector is not a true vector, bool is not a simple type +template <> +struct IsFixedSize : public TrueType {}; + +} // namespace message_traits +} // namespace ros + +#endif // __ROSNOROS__BUILTIN_MESSAGE_TRAITS_H__ diff --git a/fpsdk_common/rosnoros/ros/message_operations.h b/fpsdk_common/rosnoros/ros/message_operations.h new file mode 100644 index 00000000..21e383c3 --- /dev/null +++ b/fpsdk_common/rosnoros/ros/message_operations.h @@ -0,0 +1,86 @@ +/* + * 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 utils polyfill + * + * @details Files impoorted: + * - ros/message_operations.h + * + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * + */ + +#ifndef __ROSNOROS__MESSAGE_OPERATIONS_H__ +#define __ROSNOROS__MESSAGE_OPERATIONS_H__ + +/**********************************************************************************************************************/ +// ros/message_operations.h +/**********************************************************************************************************************/ +#include // IWYU pragma: keep + +namespace ros { +namespace message_operations { + +template +struct Printer { + template + static void stream(Stream& s, const std::string& indent, const M& value) { + (void)indent; + s << value << "\n"; + } +}; + +// Explicitly specialize for uint8_t/int8_t because otherwise it thinks it's a char, and treats +// the value as a character code +template <> +struct Printer { + template + static void stream(Stream& s, const std::string& indent, int8_t value) { + (void)indent; + s << static_cast(value) << "\n"; + } +}; + +template <> +struct Printer { + template + static void stream(Stream& s, const std::string& indent, uint8_t value) { + (void)indent; + s << static_cast(value) << "\n"; + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // __ROSNOROS__MESSAGE_OPERATIONS_H__ diff --git a/fpsdk_common/rosnoros/ros/serialization.h b/fpsdk_common/rosnoros/ros/serialization.h new file mode 100644 index 00000000..e9ba683b --- /dev/null +++ b/fpsdk_common/rosnoros/ros/serialization.h @@ -0,0 +1,814 @@ +/* + * 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 message_traits polyfill + * + * @details Files impoorted: + * - ros/roscpp_serialization_macros.h + * - ros/serialized_message.h + * - ros/serialization.h + * + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * + */ + +#ifndef __ROSNOROS__SERIALIZATION_H__ +#define __ROSNOROS__SERIALIZATION_H__ + +/**********************************************************************************************************************/ +// ros/roscpp_serialization_macros.h +/**********************************************************************************************************************/ +#define ROSCPP_SERIALIZATION_DECL + +/**********************************************************************************************************************/ +// ros/serialized_message.h +/**********************************************************************************************************************/ +#include +#include + +namespace ros { + +class ROSCPP_SERIALIZATION_DECL SerializedMessage { + public: + boost::shared_array buf; + size_t num_bytes; + uint8_t* message_start; + + boost::shared_ptr message; + const std::type_info* type_info; + + SerializedMessage() : buf(boost::shared_array()), num_bytes(0), message_start(0), type_info(0) {} + + SerializedMessage(boost::shared_array buf, size_t num_bytes) + : buf(buf), num_bytes(num_bytes), message_start(buf ? buf.get() : 0), type_info(0) {} +}; + +} // namespace ros + +/**********************************************************************************************************************/ +// ros/serialization.h +/**********************************************************************************************************************/ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "builtin_message_traits.h" +#include "time.h" +#include "utils.h" + +#define ROS_NEW_SERIALIZATION_API 1 +#define ROS_FORCE_INLINE inline + +/** + * \brief Declare your serializer to use an allInOne member instead of requiring 3 different serialization + * functions. + * + * The allinone method has the form: +\verbatim +template +inline static void allInOne(Stream& stream, T t) +{ + stream.next(t.a); + stream.next(t.b); + ... +} +\endverbatim + * + * The only guarantee given is that Stream::next(T) is defined. + */ +#define ROS_DECLARE_ALLINONE_SERIALIZER \ + template \ + inline static void write(Stream& stream, const T& t) { \ + allInOne(stream, t); \ + } \ + \ + template \ + inline static void read(Stream& stream, T& t) { \ + allInOne(stream, t); \ + } \ + \ + template \ + inline static uint32_t serializedLength(const T& t) { \ + LStream stream; \ + allInOne(stream, t); \ + return stream.getLength(); \ + } + +namespace ros { +namespace serialization { +namespace mt = message_traits; +namespace mpl = boost::mpl; + +class ROSCPP_SERIALIZATION_DECL StreamOverrunException : public ros::Exception { + public: + StreamOverrunException(const std::string& what) : Exception(what) {} +}; + +inline void throwStreamOverrun() { throw StreamOverrunException("Buffer Overrun"); } + +/** + * \brief Templated serialization class. Default implementation provides backwards compatibility with + * old message types. + * + * Specializing the Serializer class is the only thing you need to do to get the ROS serialization system + * to work with a type. + */ +template +struct Serializer { + /** + * \brief Write an object to the stream. Normally the stream passed in here will be a ros::serialization::OStream + */ + template + inline static void write(Stream& stream, typename boost::call_traits::param_type t) { + t.serialize(stream.getData(), 0); + } + + /** + * \brief Read an object from the stream. Normally the stream passed in here will be a ros::serialization::IStream + */ + template + inline static void read(Stream& stream, typename boost::call_traits::reference t) { + t.deserialize(stream.getData()); + } + + /** + * \brief Determine the serialized length of an object. + */ + inline static uint32_t serializedLength(typename boost::call_traits::param_type t) { + return t.serializationLength(); + } +}; + +/** + * \brief Serialize an object. Stream here should normally be a ros::serialization::OStream + */ +template +inline void serialize(Stream& stream, const T& t) { + Serializer::write(stream, t); +} + +/** + * \brief Deserialize an object. Stream here should normally be a ros::serialization::IStream + */ +template +inline void deserialize(Stream& stream, T& t) { + Serializer::read(stream, t); +} + +// Circumvent bug https://github.com/ros/roscpp_core/issues/130 which manifests only on ARM GCC 9.3 +#if defined(__aarch64__) && __GNUC__ == 9 && __GNUC_MINOR__ == 3 +#define ROS_SERIALIZATION_GCC_9_3_DISABLE_VECTORIZE __attribute__((optimize("no-tree-vectorize"))) +#else +#define ROS_SERIALIZATION_GCC_9_3_DISABLE_VECTORIZE +#endif + +/** + * \brief Determine the serialized length of an object + */ +template +inline uint32_t ROS_SERIALIZATION_GCC_9_3_DISABLE_VECTORIZE serializationLength(const T& t) { + return Serializer::serializedLength(t); +} + +#define ROS_CREATE_SIMPLE_SERIALIZER(Type) \ + template <> \ + struct Serializer { \ + template \ + inline static void write(Stream& stream, const Type v) { \ + memcpy(stream.advance(sizeof(v)), &v, sizeof(v)); \ + } \ + \ + template \ + inline static void read(Stream& stream, Type& v) { \ + memcpy(&v, stream.advance(sizeof(v)), sizeof(v)); \ + } \ + \ + inline static uint32_t serializedLength(const Type&) { return sizeof(Type); } \ + }; + +ROS_CREATE_SIMPLE_SERIALIZER(uint8_t) +ROS_CREATE_SIMPLE_SERIALIZER(int8_t) +ROS_CREATE_SIMPLE_SERIALIZER(uint16_t) +ROS_CREATE_SIMPLE_SERIALIZER(int16_t) +ROS_CREATE_SIMPLE_SERIALIZER(uint32_t) +ROS_CREATE_SIMPLE_SERIALIZER(int32_t) +ROS_CREATE_SIMPLE_SERIALIZER(uint64_t) +ROS_CREATE_SIMPLE_SERIALIZER(int64_t) +ROS_CREATE_SIMPLE_SERIALIZER(float) +ROS_CREATE_SIMPLE_SERIALIZER(double) + +/** + * \brief Serializer specialized for bool (serialized as uint8) + */ +template <> +struct Serializer { + template + inline static void write(Stream& stream, const bool v) { + uint8_t b = static_cast(v); + memcpy(stream.advance(1), &b, 1); + } + + template + inline static void read(Stream& stream, bool& v) { + uint8_t b; + memcpy(&b, stream.advance(1), 1); + v = static_cast(b); + } + + inline static uint32_t serializedLength(bool) { return 1; } +}; + +/** + * \brief Serializer specialized for std::string + */ +template +struct Serializer, ContainerAllocator>> { + typedef std::basic_string, ContainerAllocator> StringType; + + template + inline static void write(Stream& stream, const StringType& str) { + size_t len = str.size(); + stream.next(static_cast(len)); + + if (len > 0) { + memcpy(stream.advance(static_cast(len)), str.data(), len); + } + } + + template + inline static void read(Stream& stream, StringType& str) { + uint32_t len; + stream.next(len); + if (len > 0) { + str = StringType(reinterpret_cast(stream.advance(len)), len); + } else { + str.clear(); + } + } + + inline static uint32_t serializedLength(const StringType& str) { return 4 + static_cast(str.size()); } +}; + +/** + * \brief Serializer specialized for ros::Time + */ +template <> +struct Serializer { + template + inline static void write(Stream& stream, const ros::Time& v) { + stream.next(v.sec); + stream.next(v.nsec); + } + + template + inline static void read(Stream& stream, ros::Time& v) { + stream.next(v.sec); + stream.next(v.nsec); + } + + inline static uint32_t serializedLength(const ros::Time&) { return 8; } +}; + +/** + * \brief Serializer specialized for ros::Duration + */ +template <> +struct Serializer { + template + inline static void write(Stream& stream, const ros::Duration& v) { + stream.next(v.sec); + stream.next(v.nsec); + } + + template + inline static void read(Stream& stream, ros::Duration& v) { + stream.next(v.sec); + stream.next(v.nsec); + } + + inline static uint32_t serializedLength(const ros::Duration&) { return 8; } +}; + +/** + * \brief Vector serializer. Default implementation does nothing + */ +template +struct VectorSerializer {}; + +/** + * \brief Vector serializer, specialized for non-fixed-size, non-simple types + */ +template +struct VectorSerializer>::type> { + typedef std::vector::template rebind_alloc> VecType; + typedef typename VecType::iterator IteratorType; + typedef typename VecType::const_iterator ConstIteratorType; + + template + inline static void write(Stream& stream, const VecType& v) { + stream.next(static_cast(v.size())); + ConstIteratorType it = v.begin(); + ConstIteratorType end = v.end(); + for (; it != end; ++it) { + stream.next(*it); + } + } + + template + inline static void read(Stream& stream, VecType& v) { + uint32_t len; + stream.next(len); + v.resize(len); + IteratorType it = v.begin(); + IteratorType end = v.end(); + for (; it != end; ++it) { + stream.next(*it); + } + } + + inline static uint32_t serializedLength(const VecType& v) { + uint32_t size = 4; + ConstIteratorType it = v.begin(); + ConstIteratorType end = v.end(); + for (; it != end; ++it) { + size += serializationLength(*it); + } + + return size; + } +}; + +/** + * \brief Vector serializer, specialized for fixed-size simple types + */ +template +struct VectorSerializer>::type> { + typedef std::vector::template rebind_alloc> VecType; + typedef typename VecType::iterator IteratorType; + typedef typename VecType::const_iterator ConstIteratorType; + + template + inline static void write(Stream& stream, const VecType& v) { + uint32_t len = static_cast(v.size()); + stream.next(len); + if (!v.empty()) { + const uint32_t data_len = len * static_cast(sizeof(T)); + memcpy(stream.advance(data_len), &v.front(), data_len); + } + } + + template + inline static void read(Stream& stream, VecType& v) { + uint32_t len; + stream.next(len); + v.resize(len); + + if (len > 0) { + const uint32_t data_len = static_cast(sizeof(T)) * len; + memcpy(static_cast(&v.front()), stream.advance(data_len), data_len); + } + } + + inline static uint32_t serializedLength(const VecType& v) { + return 4 + v.size() * static_cast(sizeof(T)); + } +}; + +/** + * \brief Vector serializer, specialized for fixed-size non-simple types + */ +template +struct VectorSerializer, mpl::not_>>>::type> { + typedef std::vector::template rebind_alloc> VecType; + typedef typename VecType::iterator IteratorType; + typedef typename VecType::const_iterator ConstIteratorType; + + template + inline static void write(Stream& stream, const VecType& v) { + stream.next(static_cast(v.size())); + ConstIteratorType it = v.begin(); + ConstIteratorType end = v.end(); + for (; it != end; ++it) { + stream.next(*it); + } + } + + template + inline static void read(Stream& stream, VecType& v) { + uint32_t len; + stream.next(len); + v.resize(len); + IteratorType it = v.begin(); + IteratorType end = v.end(); + for (; it != end; ++it) { + stream.next(*it); + } + } + + inline static uint32_t serializedLength(const VecType& v) { + uint32_t size = 4; + if (!v.empty()) { + uint32_t len_each = serializationLength(v.front()); + size += len_each * static_cast(v.size()); + } + + return size; + } +}; + +/** + * \brief serialize version for std::vector + */ +template +inline void serialize(Stream& stream, const std::vector& t) { + VectorSerializer::write(stream, t); +} + +/** + * \brief deserialize version for std::vector + */ +template +inline void deserialize(Stream& stream, std::vector& t) { + VectorSerializer::read(stream, t); +} + +/** + * \brief serializationLength version for std::vector + */ +template +inline uint32_t serializationLength(const std::vector& t) { + return VectorSerializer::serializedLength(t); +} + +/** + * \brief Array serializer, default implementation does nothing + */ +template +struct ArraySerializer {}; + +/** + * \brief Array serializer, specialized for non-fixed-size, non-simple types + */ +template +struct ArraySerializer>::type> { + typedef boost::array ArrayType; + typedef typename ArrayType::iterator IteratorType; + typedef typename ArrayType::const_iterator ConstIteratorType; + + template + inline static void write(Stream& stream, const ArrayType& v) { + ConstIteratorType it = v.begin(); + ConstIteratorType end = v.end(); + for (; it != end; ++it) { + stream.next(*it); + } + } + + template + inline static void read(Stream& stream, ArrayType& v) { + IteratorType it = v.begin(); + IteratorType end = v.end(); + for (; it != end; ++it) { + stream.next(*it); + } + } + + inline static uint32_t serializedLength(const ArrayType& v) { + uint32_t size = 0; + ConstIteratorType it = v.begin(); + ConstIteratorType end = v.end(); + for (; it != end; ++it) { + size += serializationLength(*it); + } + + return size; + } +}; + +/** + * \brief Array serializer, specialized for fixed-size, simple types + */ +template +struct ArraySerializer>::type> { + typedef boost::array ArrayType; + typedef typename ArrayType::iterator IteratorType; + typedef typename ArrayType::const_iterator ConstIteratorType; + + template + inline static void write(Stream& stream, const ArrayType& v) { + const uint32_t data_len = N * sizeof(T); + memcpy(stream.advance(data_len), &v.front(), data_len); + } + + template + inline static void read(Stream& stream, ArrayType& v) { + const uint32_t data_len = N * sizeof(T); + memcpy(&v.front(), stream.advance(data_len), data_len); + } + + inline static uint32_t serializedLength(const ArrayType&) { return N * sizeof(T); } +}; + +/** + * \brief Array serializer, specialized for fixed-size, non-simple types + */ +template +struct ArraySerializer, mpl::not_>>>::type> { + typedef boost::array ArrayType; + typedef typename ArrayType::iterator IteratorType; + typedef typename ArrayType::const_iterator ConstIteratorType; + + template + inline static void write(Stream& stream, const ArrayType& v) { + ConstIteratorType it = v.begin(); + ConstIteratorType end = v.end(); + for (; it != end; ++it) { + stream.next(*it); + } + } + + template + inline static void read(Stream& stream, ArrayType& v) { + IteratorType it = v.begin(); + IteratorType end = v.end(); + for (; it != end; ++it) { + stream.next(*it); + } + } + + inline static uint32_t serializedLength(const ArrayType& v) { return serializationLength(v.front()) * N; } +}; + +/** + * \brief serialize version for boost::array + */ +template +inline void serialize(Stream& stream, const boost::array& t) { + ArraySerializer::write(stream, t); +} + +/** + * \brief deserialize version for boost::array + */ +template +inline void deserialize(Stream& stream, boost::array& t) { + ArraySerializer::read(stream, t); +} + +/** + * \brief serializationLength version for boost::array + */ +template +inline uint32_t serializationLength(const boost::array& t) { + return ArraySerializer::serializedLength(t); +} + +/** + * \brief Enum + */ +namespace stream_types { +enum StreamType { Input, Output, Length }; +} +typedef stream_types::StreamType StreamType; + +/** + * \brief Stream base-class, provides common functionality for IStream and OStream + */ +struct ROSCPP_SERIALIZATION_DECL Stream { + /* + * \brief Returns a pointer to the current position of the stream + */ + inline uint8_t* getData() { return data_; } + /** + * \brief Advances the stream, checking bounds, and returns a pointer to the position before it + * was advanced. + * \throws StreamOverrunException if len would take this stream past the end of its buffer + */ + ROS_FORCE_INLINE uint8_t* advance(uint32_t len) { + uint8_t* old_data = data_; + data_ += len; + if (data_ > end_) { + // Throwing directly here causes a significant speed hit due to the extra code generated + // for the throw statement + throwStreamOverrun(); + } + return old_data; + } + + /** + * \brief Returns the amount of space left in the stream + */ + inline uint32_t getLength() { return static_cast(end_ - data_); } + + protected: + Stream(uint8_t* _data, uint32_t _count) : data_(_data), end_(_data + _count) {} + + private: + uint8_t* data_; + uint8_t* end_; +}; + +/** + * \brief Input stream + */ +struct ROSCPP_SERIALIZATION_DECL IStream : public Stream { + static const StreamType stream_type = stream_types::Input; + + IStream(uint8_t* data, uint32_t count) : Stream(data, count) {} + + /** + * \brief Deserialize an item from this input stream + */ + template + ROS_FORCE_INLINE void next(T& t) { + deserialize(*this, t); + } + + template + ROS_FORCE_INLINE IStream& operator>>(T& t) { + deserialize(*this, t); + return *this; + } +}; + +/** + * \brief Output stream + */ +struct ROSCPP_SERIALIZATION_DECL OStream : public Stream { + static const StreamType stream_type = stream_types::Output; + + OStream(uint8_t* data, uint32_t count) : Stream(data, count) {} + + /** + * \brief Serialize an item to this output stream + */ + template + ROS_FORCE_INLINE void next(const T& t) { + serialize(*this, t); + } + + template + ROS_FORCE_INLINE OStream& operator<<(const T& t) { + serialize(*this, t); + return *this; + } +}; + +/** + * \brief Length stream + * + * LStream is not what you would normally think of as a stream, but it is used in order to support + * allinone serializers. + */ +struct ROSCPP_SERIALIZATION_DECL LStream { + static const StreamType stream_type = stream_types::Length; + + LStream() : count_(0) {} + + /** + * \brief Add the length of an item to this length stream + */ + template + ROS_FORCE_INLINE void next(const T& t) { + count_ += serializationLength(t); + } + + /** + * \brief increment the length by len + */ + ROS_FORCE_INLINE uint32_t advance(uint32_t len) { + uint32_t old = count_; + count_ += len; + return old; + } + + /** + * \brief Get the total length of this tream + */ + inline uint32_t getLength() { return count_; } + + private: + uint32_t count_; +}; + +/** + * \brief Serialize a message + */ +template +inline SerializedMessage serializeMessage(const M& message) { + SerializedMessage m; + uint32_t len = serializationLength(message); + m.num_bytes = len + 4; + m.buf.reset(new uint8_t[m.num_bytes]); + + OStream s(m.buf.get(), static_cast(m.num_bytes)); + serialize(s, static_cast(m.num_bytes) - 4); + m.message_start = s.getData(); + serialize(s, message); + + return m; +} + +/** + * \brief Serialize a service response + */ +template +inline SerializedMessage serializeServiceResponse(bool ok, const M& message) { + SerializedMessage m; + + if (ok) { + uint32_t len = serializationLength(message); + m.num_bytes = len + 5; + m.buf.reset(new uint8_t[m.num_bytes]); + + OStream s(m.buf.get(), static_cast(m.num_bytes)); + serialize(s, static_cast(ok)); + serialize(s, static_cast(m.num_bytes) - 5); + serialize(s, message); + } else { + uint32_t len = serializationLength(message); + m.num_bytes = len + 1; + m.buf.reset(new uint8_t[m.num_bytes]); + + // The intended use for failure is serializeServiceResponse(false, 0); + // The 0 written here as a message is read as message length in the deserialization + // code in ServiceServerLink::onResponseOkAndLength(). Although this is a little + // misuse of the API, it works. + OStream s(m.buf.get(), static_cast(m.num_bytes)); + serialize(s, static_cast(ok)); + serialize(s, message); + } + + return m; +} + +/** + * \brief Deserialize a message. If includes_length is true, skips the first 4 bytes + */ +template +inline void deserializeMessage(const SerializedMessage& m, M& message) { + IStream s(m.message_start, static_cast(m.num_bytes - (m.message_start - m.buf.get()))); + deserialize(s, message); +} + +// Additional serialization traits + +template +struct PreDeserializeParams { + boost::shared_ptr message; + boost::shared_ptr> connection_header; +}; + +/** + * \brief called by the SubscriptionCallbackHelper after a message is + * instantiated but before that message is deserialized + */ +template +struct PreDeserialize { + static void notify(const PreDeserializeParams&) {} +}; + +} // namespace serialization +} // namespace ros + +#endif // __ROSNOROS__SERIALIZATION_H__ diff --git a/fpsdk_common/rosnoros/ros/time.h b/fpsdk_common/rosnoros/ros/time.h new file mode 100644 index 00000000..ca18a423 --- /dev/null +++ b/fpsdk_common/rosnoros/ros/time.h @@ -0,0 +1,445 @@ +/* + * 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 + * + * @details Files impoorted: + * - ros/duration.h + * - ros/time.h + * + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * + */ + +#ifndef __ROSNOROS__TIME_H__ +#define __ROSNOROS__TIME_H__ + +/**********************************************************************************************************************/ +// COMMON +/**********************************************************************************************************************/ +#include +#include +#include +#include +#include + +namespace boost { +namespace posix_time { +class ptime; +class time_duration; +} // namespace posix_time +} // namespace boost + +#define ROSTIME_DECL + +/**********************************************************************************************************************/ +// ros/duration.h +/**********************************************************************************************************************/ +namespace ros { + +ROSTIME_DECL void normalizeSecNSecSigned(int64_t& sec, int64_t& nsec); +ROSTIME_DECL void normalizeSecNSecSigned(int32_t& sec, int32_t& nsec); + +/** + * \brief Base class for Duration implementations. Provides storage, common functions and operator overloads. + * This should not need to be used directly. + */ +template +class DurationBase { + public: + int32_t sec, nsec; + DurationBase() : sec(0), nsec(0) {} + DurationBase(int32_t _sec, int32_t _nsec); + explicit DurationBase(double t) { fromSec(t); }; + T operator+(const T& rhs) const; + T operator-(const T& rhs) const; + T operator-() const; + T operator*(double scale) const; + T& operator+=(const T& rhs); + T& operator-=(const T& rhs); + T& operator*=(double scale); + bool operator==(const T& rhs) const; + inline bool operator!=(const T& rhs) const { return !(*static_cast(this) == rhs); } + bool operator>(const T& rhs) const; + bool operator<(const T& rhs) const; + bool operator>=(const T& rhs) const; + bool operator<=(const T& rhs) const; + double toSec() const { return static_cast(sec) + 1e-9 * static_cast(nsec); }; + int64_t toNSec() const { return static_cast(sec) * 1000000000ll + static_cast(nsec); }; + T& fromSec(double t); + T& fromNSec(int64_t t); + bool isZero() const; + boost::posix_time::time_duration toBoost() const; + static const T MIN; //!< Minimum representable duration (negative) + static const T MAX; //!< Maximum representable duration + static const T ZERO; //!< Zero duration + static const T DAY; //!< One day duration + static const T HOUR; //!< One hour duration + static const T MINUTE; //!< One minute duration + static const T SECOND; //!< One second duration + static const T MILLISECOND; //!< One millisecond duration + static const T MICROSECOND; //!< One microsecond duration + static const T NANOSECOND; //!< One nanosecond duration +}; + +class Rate; + +/** + * \brief Duration representation for use with the Time class. + * + * ros::DurationBase provides most of its functionality. + */ +class ROSTIME_DECL Duration : public DurationBase { + public: + Duration() : DurationBase() {} + + Duration(int32_t _sec, int32_t _nsec) : DurationBase(_sec, _nsec) {} + + explicit Duration(double t) { fromSec(t); } + explicit Duration(const Rate&); + /** + * \brief sleep for the amount of time specified by this Duration. If a signal interrupts the sleep, resleeps for + * the time remaining. + * @return True if the desired sleep duration was met, false otherwise. + */ + bool sleep() const; +}; + +extern ROSTIME_DECL const Duration DURATION_MAX; +extern ROSTIME_DECL const Duration DURATION_MIN; +template <> +const Duration DurationBase::MAX; +template <> +const Duration DurationBase::MIN; +template <> +const Duration DurationBase::ZERO; +template <> +const Duration DurationBase::DAY; +template <> +const Duration DurationBase::HOUR; +template <> +const Duration DurationBase::MINUTE; +template <> +const Duration DurationBase::SECOND; +template <> +const Duration DurationBase::MILLISECOND; +template <> +const Duration DurationBase::MICROSECOND; +template <> +const Duration DurationBase::NANOSECOND; + +/** + * \brief Duration representation for use with the WallTime class. + * + * ros::DurationBase provides most of its functionality. + */ +class ROSTIME_DECL WallDuration : public DurationBase { + public: + WallDuration() : DurationBase() {} + + WallDuration(int32_t _sec, int32_t _nsec) : DurationBase(_sec, _nsec) {} + + explicit WallDuration(double t) { fromSec(t); } + explicit WallDuration(const Rate&); + /** + * \brief sleep for the amount of time specified by this Duration. If a signal interrupts the sleep, resleeps for + * the time remaining. + * @return True if the desired sleep duration was met, false otherwise. + */ + bool sleep() const; +}; + +template <> +const WallDuration DurationBase::MAX; +template <> +const WallDuration DurationBase::MIN; +template <> +const WallDuration DurationBase::ZERO; +template <> +const WallDuration DurationBase::DAY; +template <> +const WallDuration DurationBase::HOUR; +template <> +const WallDuration DurationBase::MINUTE; +template <> +const WallDuration DurationBase::SECOND; +template <> +const WallDuration DurationBase::MILLISECOND; +template <> +const WallDuration DurationBase::MICROSECOND; +template <> +const WallDuration DurationBase::NANOSECOND; + +ROSTIME_DECL std::ostream& operator<<(std::ostream& os, const Duration& rhs); +ROSTIME_DECL std::ostream& operator<<(std::ostream& os, const WallDuration& rhs); + +} // namespace ros + +/**********************************************************************************************************************/ +// ros/time.h +/**********************************************************************************************************************/ +#include "utils.h" + +namespace ros { + +/********************************************************************* + ** Exceptions + *********************************************************************/ + +/** + * @brief Thrown if the ros subsystem hasn't been initialised before use. + */ +class ROSTIME_DECL TimeNotInitializedException : public Exception { + public: + TimeNotInitializedException() + : Exception( + "Cannot use ros::Time::now() before the first NodeHandle has " + "been created or ros::start() has been called. " + "If this is a standalone app or test that just uses " + "ros::Time and does not communicate over ROS, you may also " + "call ros::Time::init()") {} +}; + +/** + * @brief Thrown if windows high perf. timestamping is unavailable. + * + * @sa getWallTime + */ +class ROSTIME_DECL NoHighPerformanceTimersException : public Exception { + public: + NoHighPerformanceTimersException() + : Exception( + "This windows platform does not " + "support the high-performance timing api.") {} +}; + +/********************************************************************* + ** Functions + *********************************************************************/ + +ROSTIME_DECL void normalizeSecNSec(uint64_t& sec, uint64_t& nsec); +ROSTIME_DECL void normalizeSecNSec(uint32_t& sec, uint32_t& nsec); +ROSTIME_DECL void normalizeSecNSecUnsigned(int64_t& sec, int64_t& nsec); +ROSTIME_DECL void ros_walltime(uint32_t& sec, uint32_t& nsec); +ROSTIME_DECL void ros_steadytime(uint32_t& sec, uint32_t& nsec); + +/********************************************************************* + ** Time Classes + *********************************************************************/ + +/** + * \brief Base class for Time implementations. Provides storage, common + * functions and operator overloads. This should not need to be used directly. + */ +template +class TimeBase { + public: + uint32_t sec, nsec; + + TimeBase() : sec(0), nsec(0) {} + TimeBase(uint32_t _sec, uint32_t _nsec) : sec(_sec), nsec(_nsec) { normalizeSecNSec(sec, nsec); } + explicit TimeBase(double t) { fromSec(t); } + D operator-(const T& rhs) const; + T operator+(const D& rhs) const; + T operator-(const D& rhs) const; + T& operator+=(const D& rhs); + T& operator-=(const D& rhs); + bool operator==(const T& rhs) const; + inline bool operator!=(const T& rhs) const { return !(*static_cast(this) == rhs); } + bool operator>(const T& rhs) const; + bool operator<(const T& rhs) const; + bool operator>=(const T& rhs) const; + bool operator<=(const T& rhs) const; + + double toSec() const { return static_cast(sec) + 1e-9 * static_cast(nsec); }; + T& fromSec(double t); + + uint64_t toNSec() const { return static_cast(sec) * 1000000000ull + static_cast(nsec); } + T& fromNSec(uint64_t t); + + inline bool isZero() const { return sec == 0 && nsec == 0; } + inline bool is_zero() const { return isZero(); } + boost::posix_time::ptime toBoost() const; + + static const T MIN; //!< Minimum representable time + static const T MAX; //!< Maximum representable time + static const T ZERO; //!< Zero (invalid) time + static const T UNINITIALIZED; //!< Uninitialized time +}; + +/** + * \brief Time representation. May either represent wall clock time or ROS + * clock time. + * + * ros::TimeBase provides most of its functionality. + */ +class ROSTIME_DECL Time : public TimeBase { + public: + Time() : TimeBase() {} + + Time(uint32_t _sec, uint32_t _nsec) : TimeBase(_sec, _nsec) {} + + explicit Time(double t) { fromSec(t); } + + /** + * \brief Retrieve the current time. If ROS clock time is in use, this + * returns the time according to the ROS clock. Otherwise returns the current + * wall clock time. + */ + static Time now(); + /** + * \brief Sleep until a specific time has been reached. + * @return True if the desired sleep time was met, false otherwise. + */ + static bool sleepUntil(const Time& end); + + static void init(); + static void shutdown(); + static void setNow(const Time& new_now); + static bool useSystemTime(); + static bool isSimTime(); + static bool isSystemTime(); + + /** + * \brief Returns whether or not the current time source is valid. Simulation + * time is valid if it is non-zero. + */ + static bool isValid(); + /** + * \brief Wait for time source to become valid + */ + static bool waitForValid(); + /** + * \brief Wait for time source to become valid, with timeout + */ + static bool waitForValid(const ros::WallDuration& timeout); + + static Time fromBoost(const boost::posix_time::ptime& t); + static Time fromBoost(const boost::posix_time::time_duration& d); +}; + +extern ROSTIME_DECL const Time TIME_MAX; +extern ROSTIME_DECL const Time TIME_MIN; +template <> +const Time TimeBase::MAX; +template <> +const Time TimeBase::MIN; +template <> +const Time TimeBase::ZERO; +template <> +const Time TimeBase::UNINITIALIZED; + +/** + * \brief Time representation. Always wall-clock time. + * + * ros::TimeBase provides most of its functionality. + */ +class ROSTIME_DECL WallTime : public TimeBase { + public: + WallTime() : TimeBase() {} + + WallTime(uint32_t _sec, uint32_t _nsec) : TimeBase(_sec, _nsec) {} + + explicit WallTime(double t) { fromSec(t); } + + /** + * \brief Returns the current wall clock time. + */ + static WallTime now(); + + /** + * \brief Sleep until a specific time has been reached. + * @return True if the desired sleep time was met, false otherwise. + */ + static bool sleepUntil(const WallTime& end); + + static bool isSystemTime() { return true; } +}; + +template <> +const WallTime TimeBase::MAX; +template <> +const WallTime TimeBase::MIN; +template <> +const WallTime TimeBase::ZERO; +template <> +const WallTime TimeBase::UNINITIALIZED; + +/** + * \brief Time representation. Always steady-clock time. + * + * Not affected by ROS time. + * + * ros::TimeBase provides most of its functionality. + */ +class ROSTIME_DECL SteadyTime : public TimeBase { + public: + SteadyTime() : TimeBase() {} + + SteadyTime(uint32_t _sec, uint32_t _nsec) : TimeBase(_sec, _nsec) {} + + explicit SteadyTime(double t) { fromSec(t); } + + /** + * \brief Returns the current steady (monotonic) clock time. + */ + static SteadyTime now(); + + /** + * \brief Sleep until a specific time has been reached. + * @return True if the desired sleep time was met, false otherwise. + */ + static bool sleepUntil(const SteadyTime& end); + + static bool isSystemTime() { return true; } +}; + +template <> +const SteadyTime TimeBase::MAX; +template <> +const SteadyTime TimeBase::MIN; +template <> +const SteadyTime TimeBase::ZERO; +template <> +const SteadyTime TimeBase::UNINITIALIZED; + +ROSTIME_DECL std::ostream& operator<<(std::ostream& os, const Time& rhs); +ROSTIME_DECL std::ostream& operator<<(std::ostream& os, const WallTime& rhs); +ROSTIME_DECL std::ostream& operator<<(std::ostream& os, const SteadyTime& rhs); +} // namespace ros + +/**********************************************************************************************************************/ +// ros/....h +/**********************************************************************************************************************/ + +#endif // __ROSNOROS__TIME_H__ diff --git a/fpsdk_common/rosnoros/ros/types.h b/fpsdk_common/rosnoros/ros/types.h new file mode 100644 index 00000000..dcc9df16 --- /dev/null +++ b/fpsdk_common/rosnoros/ros/types.h @@ -0,0 +1,51 @@ +/* + * 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 utils polyfill + * + * @details Files impoorted: + * - ros/types.hpp + * + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * + */ + +#ifndef __ROSNOROS__TYPES_H__ +#define __ROSNOROS__TYPES_H__ + +/**********************************************************************************************************************/ +// ros/types.hpp +/**********************************************************************************************************************/ +#include // IWYU pragma: keep + +#endif // __ROSNOROS__TYPES_H__ diff --git a/fpsdk_common/rosnoros/ros/utils.h b/fpsdk_common/rosnoros/ros/utils.h new file mode 100644 index 00000000..814b6232 --- /dev/null +++ b/fpsdk_common/rosnoros/ros/utils.h @@ -0,0 +1,108 @@ +/* + * 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 utils polyfill + * + * @details Files impoorted: + * - ros/exception.h + * - ros/platform.h + * - ros/datatypes.h + * + * ___ ___ + * \ \ / / + * \ \/ / Fixposition AG + * / /\ \ All right reserved. + * /__/ \__\ + * + */ + +#ifndef __ROSNOROS__UTILS_H__ +#define __ROSNOROS__UTILS_H__ + +/**********************************************************************************************************************/ +// ros/exception.h +/**********************************************************************************************************************/ +#include + +namespace ros { +/** + * \brief Base class for all exceptions thrown by ROS + */ +class Exception : public std::runtime_error { + public: + Exception(const std::string& what) : std::runtime_error(what) {} +}; +} // namespace ros + +/**********************************************************************************************************************/ +// ros/platform.h +/**********************************************************************************************************************/ +#include // getenv +#include + +namespace ros { + +/** + * Convenient cross platform function for returning a std::string of an + * environment variable. + */ +inline bool get_environment_variable(std::string& str, const char* environment_variable) { + char* env_var_cstr = NULL; + env_var_cstr = getenv(environment_variable); + if (env_var_cstr) { + str = std::string(env_var_cstr); + return true; + } else { + str = std::string(""); + return false; + } +} + +} // namespace ros + +/**********************************************************************************************************************/ +// ros/datatypes.h +/**********************************************************************************************************************/ +#include +#include +#include + +namespace ros { + +typedef std::vector > VP_string; +typedef std::vector V_string; +typedef std::set S_string; +typedef std::map M_string; +typedef std::pair StringPair; + +typedef boost::shared_ptr M_stringPtr; + +} // namespace ros + +#endif // __ROSNOROS__UTILS_H__ diff --git a/fpsdk_common/rosnoros/sensor_msgs/Image.h b/fpsdk_common/rosnoros/sensor_msgs/Image.h new file mode 100644 index 00000000..ae405f15 --- /dev/null +++ b/fpsdk_common/rosnoros/sensor_msgs/Image.h @@ -0,0 +1,317 @@ +// Generated by gencpp from file sensor_msgs/Image.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_IMAGE_H +#define SENSOR_MSGS_MESSAGE_IMAGE_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace sensor_msgs +{ +template +struct Image_ +{ + typedef Image_ Type; + + Image_() + : header() + , height(0) + , width(0) + , encoding() + , is_bigendian(0) + , step(0) + , data() { + } + Image_(const ContainerAllocator& _alloc) + : header(_alloc) + , height(0) + , width(0) + , encoding(_alloc) + , is_bigendian(0) + , step(0) + , data(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef uint32_t _height_type; + _height_type height; + + typedef uint32_t _width_type; + _width_type width; + + typedef std::basic_string, typename std::allocator_traits::template rebind_alloc> _encoding_type; + _encoding_type encoding; + + typedef uint8_t _is_bigendian_type; + _is_bigendian_type is_bigendian; + + typedef uint32_t _step_type; + _step_type step; + + typedef std::vector::template rebind_alloc> _data_type; + _data_type data; + + + + + + typedef boost::shared_ptr< ::sensor_msgs::Image_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::Image_ const> ConstPtr; + +}; // struct Image_ + +typedef ::sensor_msgs::Image_ > Image; + +typedef boost::shared_ptr< ::sensor_msgs::Image > ImagePtr; +typedef boost::shared_ptr< ::sensor_msgs::Image const> ImageConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::Image_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::Image_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::sensor_msgs::Image_ & lhs, const ::sensor_msgs::Image_ & rhs) +{ + return lhs.header == rhs.header && + lhs.height == rhs.height && + lhs.width == rhs.width && + lhs.encoding == rhs.encoding && + lhs.is_bigendian == rhs.is_bigendian && + lhs.step == rhs.step && + lhs.data == rhs.data; +} + +template +bool operator!=(const ::sensor_msgs::Image_ & lhs, const ::sensor_msgs::Image_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::sensor_msgs::Image_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::Image_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::sensor_msgs::Image_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::Image_ const> + : FalseType + { }; + +template +struct HasHeader< ::sensor_msgs::Image_ > + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::Image_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::sensor_msgs::Image_ > +{ + static const char* value() + { + return "060021388200f6f0f447d0fcd9c64743"; + } + + static const char* value(const ::sensor_msgs::Image_&) { return value(); } + static const uint64_t static_value1 = 0x060021388200f6f0ULL; + static const uint64_t static_value2 = 0xf447d0fcd9c64743ULL; +}; + +template +struct DataType< ::sensor_msgs::Image_ > +{ + static const char* value() + { + return "sensor_msgs/Image"; + } + + static const char* value(const ::sensor_msgs::Image_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::Image_ > +{ + static const char* value() + { + return "# This message contains an uncompressed image\n" +"# (0, 0) is at top-left corner of image\n" +"#\n" +"\n" +"Header header # Header timestamp should be acquisition time of image\n" +" # Header frame_id should be optical frame of camera\n" +" # origin of frame should be optical center of camera\n" +" # +x should point to the right in the image\n" +" # +y should point down in the image\n" +" # +z should point into to plane of the image\n" +" # If the frame_id here and the frame_id of the CameraInfo\n" +" # message associated with the image conflict\n" +" # the behavior is undefined\n" +"\n" +"uint32 height # image height, that is, number of rows\n" +"uint32 width # image width, that is, number of columns\n" +"\n" +"# The legal values for encoding are in file src/image_encodings.cpp\n" +"# If you want to standardize a new string format, join\n" +"# ros-users@lists.sourceforge.net and send an email proposing a new encoding.\n" +"\n" +"string encoding # Encoding of pixels -- channel meaning, ordering, size\n" +" # taken from the list of strings in include/sensor_msgs/image_encodings.h\n" +"\n" +"uint8 is_bigendian # is this data bigendian?\n" +"uint32 step # Full row length in bytes\n" +"uint8[] data # actual matrix data, size is (step * rows)\n" +"\n" +"================================================================================\n" +"MSG: std_msgs/Header\n" +"# Standard metadata for higher-level stamped data types.\n" +"# This is generally used to communicate timestamped data \n" +"# in a particular coordinate frame.\n" +"# \n" +"# sequence ID: consecutively increasing ID \n" +"uint32 seq\n" +"#Two-integer timestamp that is expressed as:\n" +"# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n" +"# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n" +"# time-handling sugar is provided by the client library\n" +"time stamp\n" +"#Frame this data is associated with\n" +"string frame_id\n" +; + } + + static const char* value(const ::sensor_msgs::Image_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::Image_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.height); + stream.next(m.width); + stream.next(m.encoding); + stream.next(m.is_bigendian); + stream.next(m.step); + stream.next(m.data); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Image_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::Image_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::Image_& v) + { + if (false || !indent.empty()) + s << std::endl; + s << indent << "header: "; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + if (true || !indent.empty()) + s << std::endl; + s << indent << "height: "; + Printer::stream(s, indent + " ", v.height); + if (true || !indent.empty()) + s << std::endl; + s << indent << "width: "; + Printer::stream(s, indent + " ", v.width); + if (true || !indent.empty()) + s << std::endl; + s << indent << "encoding: "; + Printer, typename std::allocator_traits::template rebind_alloc>>::stream(s, indent + " ", v.encoding); + if (true || !indent.empty()) + s << std::endl; + s << indent << "is_bigendian: "; + Printer::stream(s, indent + " ", v.is_bigendian); + if (true || !indent.empty()) + s << std::endl; + s << indent << "step: "; + Printer::stream(s, indent + " ", v.step); + if (true || !indent.empty()) + s << std::endl; + s << indent << "data: "; + if (v.data.empty() || true) + s << "["; + for (size_t i = 0; i < v.data.size(); ++i) + { + if (true && i > 0) + s << ", "; + else if (!true) + s << std::endl << indent << " -"; + Printer::stream(s, true ? std::string() : indent + " ", v.data[i]); + } + if (v.data.empty() || true) + s << "]"; + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_IMAGE_H diff --git a/fpsdk_common/rosnoros/sensor_msgs/Imu.h b/fpsdk_common/rosnoros/sensor_msgs/Imu.h new file mode 100644 index 00000000..b3394500 --- /dev/null +++ b/fpsdk_common/rosnoros/sensor_msgs/Imu.h @@ -0,0 +1,340 @@ +// Generated by gencpp from file sensor_msgs/Imu.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_IMU_H +#define SENSOR_MSGS_MESSAGE_IMU_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace sensor_msgs +{ +template +struct Imu_ +{ + typedef Imu_ Type; + + Imu_() + : header() + , orientation() + , orientation_covariance() + , angular_velocity() + , angular_velocity_covariance() + , linear_acceleration() + , linear_acceleration_covariance() { + orientation_covariance.assign(0.0); + + angular_velocity_covariance.assign(0.0); + + linear_acceleration_covariance.assign(0.0); + } + Imu_(const ContainerAllocator& _alloc) + : header(_alloc) + , orientation(_alloc) + , orientation_covariance() + , angular_velocity(_alloc) + , angular_velocity_covariance() + , linear_acceleration(_alloc) + , linear_acceleration_covariance() { + (void)_alloc; + orientation_covariance.assign(0.0); + + angular_velocity_covariance.assign(0.0); + + linear_acceleration_covariance.assign(0.0); + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef ::geometry_msgs::Quaternion_ _orientation_type; + _orientation_type orientation; + + typedef boost::array _orientation_covariance_type; + _orientation_covariance_type orientation_covariance; + + typedef ::geometry_msgs::Vector3_ _angular_velocity_type; + _angular_velocity_type angular_velocity; + + typedef boost::array _angular_velocity_covariance_type; + _angular_velocity_covariance_type angular_velocity_covariance; + + typedef ::geometry_msgs::Vector3_ _linear_acceleration_type; + _linear_acceleration_type linear_acceleration; + + typedef boost::array _linear_acceleration_covariance_type; + _linear_acceleration_covariance_type linear_acceleration_covariance; + + + + + + typedef boost::shared_ptr< ::sensor_msgs::Imu_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::Imu_ const> ConstPtr; + +}; // struct Imu_ + +typedef ::sensor_msgs::Imu_ > Imu; + +typedef boost::shared_ptr< ::sensor_msgs::Imu > ImuPtr; +typedef boost::shared_ptr< ::sensor_msgs::Imu const> ImuConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::Imu_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::Imu_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::sensor_msgs::Imu_ & lhs, const ::sensor_msgs::Imu_ & rhs) +{ + return lhs.header == rhs.header && + lhs.orientation == rhs.orientation && + lhs.orientation_covariance == rhs.orientation_covariance && + lhs.angular_velocity == rhs.angular_velocity && + lhs.angular_velocity_covariance == rhs.angular_velocity_covariance && + lhs.linear_acceleration == rhs.linear_acceleration && + lhs.linear_acceleration_covariance == rhs.linear_acceleration_covariance; +} + +template +bool operator!=(const ::sensor_msgs::Imu_ & lhs, const ::sensor_msgs::Imu_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::sensor_msgs::Imu_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::Imu_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::sensor_msgs::Imu_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::Imu_ const> + : FalseType + { }; + +template +struct HasHeader< ::sensor_msgs::Imu_ > + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::Imu_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::sensor_msgs::Imu_ > +{ + static const char* value() + { + return "6a62c6daae103f4ff57a132d6f95cec2"; + } + + static const char* value(const ::sensor_msgs::Imu_&) { return value(); } + static const uint64_t static_value1 = 0x6a62c6daae103f4fULL; + static const uint64_t static_value2 = 0xf57a132d6f95cec2ULL; +}; + +template +struct DataType< ::sensor_msgs::Imu_ > +{ + static const char* value() + { + return "sensor_msgs/Imu"; + } + + static const char* value(const ::sensor_msgs::Imu_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::Imu_ > +{ + static const char* value() + { + return "# This is a message to hold data from an IMU (Inertial Measurement Unit)\n" +"#\n" +"# Accelerations should be in m/s^2 (not in g's), and rotational velocity should be in rad/sec\n" +"#\n" +"# If the covariance of the measurement is known, it should be filled in (if all you know is the \n" +"# variance of each measurement, e.g. from the datasheet, just put those along the diagonal)\n" +"# A covariance matrix of all zeros will be interpreted as \"covariance unknown\", and to use the\n" +"# data a covariance will have to be assumed or gotten from some other source\n" +"#\n" +"# If you have no estimate for one of the data elements (e.g. your IMU doesn't produce an orientation \n" +"# estimate), please set element 0 of the associated covariance matrix to -1\n" +"# If you are interpreting this message, please check for a value of -1 in the first element of each \n" +"# covariance matrix, and disregard the associated estimate.\n" +"\n" +"Header header\n" +"\n" +"geometry_msgs/Quaternion orientation\n" +"float64[9] orientation_covariance # Row major about x, y, z axes\n" +"\n" +"geometry_msgs/Vector3 angular_velocity\n" +"float64[9] angular_velocity_covariance # Row major about x, y, z axes\n" +"\n" +"geometry_msgs/Vector3 linear_acceleration\n" +"float64[9] linear_acceleration_covariance # Row major x, y z \n" +"\n" +"================================================================================\n" +"MSG: std_msgs/Header\n" +"# Standard metadata for higher-level stamped data types.\n" +"# This is generally used to communicate timestamped data \n" +"# in a particular coordinate frame.\n" +"# \n" +"# sequence ID: consecutively increasing ID \n" +"uint32 seq\n" +"#Two-integer timestamp that is expressed as:\n" +"# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n" +"# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n" +"# time-handling sugar is provided by the client library\n" +"time stamp\n" +"#Frame this data is associated with\n" +"string frame_id\n" +"\n" +"================================================================================\n" +"MSG: geometry_msgs/Quaternion\n" +"# This represents an orientation in free space in quaternion form.\n" +"\n" +"float64 x\n" +"float64 y\n" +"float64 z\n" +"float64 w\n" +"\n" +"================================================================================\n" +"MSG: geometry_msgs/Vector3\n" +"# This represents a vector in free space. \n" +"# It is only meant to represent a direction. Therefore, it does not\n" +"# make sense to apply a translation to it (e.g., when applying a \n" +"# generic rigid transformation to a Vector3, tf2 will only apply the\n" +"# rotation). If you want your data to be translatable too, use the\n" +"# geometry_msgs/Point message instead.\n" +"\n" +"float64 x\n" +"float64 y\n" +"float64 z\n" +; + } + + static const char* value(const ::sensor_msgs::Imu_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::Imu_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.orientation); + stream.next(m.orientation_covariance); + stream.next(m.angular_velocity); + stream.next(m.angular_velocity_covariance); + stream.next(m.linear_acceleration); + stream.next(m.linear_acceleration_covariance); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Imu_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::Imu_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::Imu_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "orientation: "; + s << std::endl; + Printer< ::geometry_msgs::Quaternion_ >::stream(s, indent + " ", v.orientation); + s << indent << "orientation_covariance[]" << std::endl; + for (size_t i = 0; i < v.orientation_covariance.size(); ++i) + { + s << indent << " orientation_covariance[" << i << "]: "; + Printer::stream(s, indent + " ", v.orientation_covariance[i]); + } + s << indent << "angular_velocity: "; + s << std::endl; + Printer< ::geometry_msgs::Vector3_ >::stream(s, indent + " ", v.angular_velocity); + s << indent << "angular_velocity_covariance[]" << std::endl; + for (size_t i = 0; i < v.angular_velocity_covariance.size(); ++i) + { + s << indent << " angular_velocity_covariance[" << i << "]: "; + Printer::stream(s, indent + " ", v.angular_velocity_covariance[i]); + } + s << indent << "linear_acceleration: "; + s << std::endl; + Printer< ::geometry_msgs::Vector3_ >::stream(s, indent + " ", v.linear_acceleration); + s << indent << "linear_acceleration_covariance[]" << std::endl; + for (size_t i = 0; i < v.linear_acceleration_covariance.size(); ++i) + { + s << indent << " linear_acceleration_covariance[" << i << "]: "; + Printer::stream(s, indent + " ", v.linear_acceleration_covariance[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_IMU_H diff --git a/fpsdk_common/rosnoros/sensor_msgs/Temperature.h b/fpsdk_common/rosnoros/sensor_msgs/Temperature.h new file mode 100644 index 00000000..1fa4f7c1 --- /dev/null +++ b/fpsdk_common/rosnoros/sensor_msgs/Temperature.h @@ -0,0 +1,237 @@ +// Generated by gencpp from file sensor_msgs/Temperature.msg +// DO NOT EDIT! + + +#ifndef SENSOR_MSGS_MESSAGE_TEMPERATURE_H +#define SENSOR_MSGS_MESSAGE_TEMPERATURE_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace sensor_msgs +{ +template +struct Temperature_ +{ + typedef Temperature_ Type; + + Temperature_() + : header() + , temperature(0.0) + , variance(0.0) { + } + Temperature_(const ContainerAllocator& _alloc) + : header(_alloc) + , temperature(0.0) + , variance(0.0) { + (void)_alloc; + } + + + + typedef ::std_msgs::Header_ _header_type; + _header_type header; + + typedef double _temperature_type; + _temperature_type temperature; + + typedef double _variance_type; + _variance_type variance; + + + + + + typedef boost::shared_ptr< ::sensor_msgs::Temperature_ > Ptr; + typedef boost::shared_ptr< ::sensor_msgs::Temperature_ const> ConstPtr; + +}; // struct Temperature_ + +typedef ::sensor_msgs::Temperature_ > Temperature; + +typedef boost::shared_ptr< ::sensor_msgs::Temperature > TemperaturePtr; +typedef boost::shared_ptr< ::sensor_msgs::Temperature const> TemperatureConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::sensor_msgs::Temperature_ & v) +{ +ros::message_operations::Printer< ::sensor_msgs::Temperature_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::sensor_msgs::Temperature_ & lhs, const ::sensor_msgs::Temperature_ & rhs) +{ + return lhs.header == rhs.header && + lhs.temperature == rhs.temperature && + lhs.variance == rhs.variance; +} + +template +bool operator!=(const ::sensor_msgs::Temperature_ & lhs, const ::sensor_msgs::Temperature_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace sensor_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::sensor_msgs::Temperature_ > + : TrueType + { }; + +template +struct IsMessage< ::sensor_msgs::Temperature_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::sensor_msgs::Temperature_ > + : FalseType + { }; + +template +struct IsFixedSize< ::sensor_msgs::Temperature_ const> + : FalseType + { }; + +template +struct HasHeader< ::sensor_msgs::Temperature_ > + : TrueType + { }; + +template +struct HasHeader< ::sensor_msgs::Temperature_ const> + : TrueType + { }; + + +template +struct MD5Sum< ::sensor_msgs::Temperature_ > +{ + static const char* value() + { + return "ff71b307acdbe7c871a5a6d7ed359100"; + } + + static const char* value(const ::sensor_msgs::Temperature_&) { return value(); } + static const uint64_t static_value1 = 0xff71b307acdbe7c8ULL; + static const uint64_t static_value2 = 0x71a5a6d7ed359100ULL; +}; + +template +struct DataType< ::sensor_msgs::Temperature_ > +{ + static const char* value() + { + return "sensor_msgs/Temperature"; + } + + static const char* value(const ::sensor_msgs::Temperature_&) { return value(); } +}; + +template +struct Definition< ::sensor_msgs::Temperature_ > +{ + static const char* value() + { + return " # Single temperature reading.\n" +"\n" +" Header header # timestamp is the time the temperature was measured\n" +" # frame_id is the location of the temperature reading\n" +"\n" +" float64 temperature # Measurement of the Temperature in Degrees Celsius\n" +"\n" +" float64 variance # 0 is interpreted as variance unknown\n" +"================================================================================\n" +"MSG: std_msgs/Header\n" +"# Standard metadata for higher-level stamped data types.\n" +"# This is generally used to communicate timestamped data \n" +"# in a particular coordinate frame.\n" +"# \n" +"# sequence ID: consecutively increasing ID \n" +"uint32 seq\n" +"#Two-integer timestamp that is expressed as:\n" +"# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n" +"# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n" +"# time-handling sugar is provided by the client library\n" +"time stamp\n" +"#Frame this data is associated with\n" +"string frame_id\n" +; + } + + static const char* value(const ::sensor_msgs::Temperature_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::sensor_msgs::Temperature_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.header); + stream.next(m.temperature); + stream.next(m.variance); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Temperature_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::sensor_msgs::Temperature_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::sensor_msgs::Temperature_& v) + { + s << indent << "header: "; + s << std::endl; + Printer< ::std_msgs::Header_ >::stream(s, indent + " ", v.header); + s << indent << "temperature: "; + Printer::stream(s, indent + " ", v.temperature); + s << indent << "variance: "; + Printer::stream(s, indent + " ", v.variance); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // SENSOR_MSGS_MESSAGE_TEMPERATURE_H diff --git a/fpsdk_common/rosnoros/sensor_msgs/image_encodings.h b/fpsdk_common/rosnoros/sensor_msgs/image_encodings.h new file mode 100644 index 00000000..d1a9c89a --- /dev/null +++ b/fpsdk_common/rosnoros/sensor_msgs/image_encodings.h @@ -0,0 +1,233 @@ + +/********************************************************************* +* Software License Agreement (BSD License) +* +* Copyright (c) 2009, Willow Garage, Inc. +* All rights reserved. +* +* Redistribution and use in source and binary forms, with or without +* modification, are permitted provided that the following conditions +* are met: +* +* * Redistributions of source code must retain the above copyright +* notice, this list of conditions and the following disclaimer. +* * Redistributions in binary form must reproduce the above +* copyright notice, this list of conditions and the following +* disclaimer in the documentation and/or other materials provided +* with the distribution. +* * Neither the name of the Willow Garage nor the names of its +* contributors may be used to endorse or promote products derived +* from this software without specific prior written permission. +* +* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; +* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +* POSSIBILITY OF SUCH DAMAGE. +*********************************************************************/ + +#ifndef SENSOR_MSGS_IMAGE_ENCODINGS_H +#define SENSOR_MSGS_IMAGE_ENCODINGS_H + +#include +#include +#include + +namespace sensor_msgs +{ + namespace image_encodings + { + const std::string RGB8 = "rgb8"; + const std::string RGBA8 = "rgba8"; + const std::string RGB16 = "rgb16"; + const std::string RGBA16 = "rgba16"; + const std::string BGR8 = "bgr8"; + const std::string BGRA8 = "bgra8"; + const std::string BGR16 = "bgr16"; + const std::string BGRA16 = "bgra16"; + const std::string MONO8="mono8"; + const std::string MONO16="mono16"; + + // OpenCV CvMat types + const std::string TYPE_8UC1="8UC1"; + const std::string TYPE_8UC2="8UC2"; + const std::string TYPE_8UC3="8UC3"; + const std::string TYPE_8UC4="8UC4"; + const std::string TYPE_8SC1="8SC1"; + const std::string TYPE_8SC2="8SC2"; + const std::string TYPE_8SC3="8SC3"; + const std::string TYPE_8SC4="8SC4"; + const std::string TYPE_16UC1="16UC1"; + const std::string TYPE_16UC2="16UC2"; + const std::string TYPE_16UC3="16UC3"; + const std::string TYPE_16UC4="16UC4"; + const std::string TYPE_16SC1="16SC1"; + const std::string TYPE_16SC2="16SC2"; + const std::string TYPE_16SC3="16SC3"; + const std::string TYPE_16SC4="16SC4"; + const std::string TYPE_32SC1="32SC1"; + const std::string TYPE_32SC2="32SC2"; + const std::string TYPE_32SC3="32SC3"; + const std::string TYPE_32SC4="32SC4"; + const std::string TYPE_32FC1="32FC1"; + const std::string TYPE_32FC2="32FC2"; + const std::string TYPE_32FC3="32FC3"; + const std::string TYPE_32FC4="32FC4"; + const std::string TYPE_64FC1="64FC1"; + const std::string TYPE_64FC2="64FC2"; + const std::string TYPE_64FC3="64FC3"; + const std::string TYPE_64FC4="64FC4"; + + // Bayer encodings + const std::string BAYER_RGGB8="bayer_rggb8"; + const std::string BAYER_BGGR8="bayer_bggr8"; + const std::string BAYER_GBRG8="bayer_gbrg8"; + const std::string BAYER_GRBG8="bayer_grbg8"; + const std::string BAYER_RGGB16="bayer_rggb16"; + const std::string BAYER_BGGR16="bayer_bggr16"; + const std::string BAYER_GBRG16="bayer_gbrg16"; + const std::string BAYER_GRBG16="bayer_grbg16"; + + // Miscellaneous + // This is the UYVY version of YUV422 codec http://www.fourcc.org/yuv.php#UYVY + // with an 8-bit depth + const std::string YUV422="yuv422"; + + // Prefixes for abstract image encodings + const std::string ABSTRACT_ENCODING_PREFIXES[] = { + "8UC", "8SC", "16UC", "16SC", "32SC", "32FC", "64FC"}; + + // Utility functions for inspecting an encoding string + static inline bool isColor(const std::string& encoding) + { + return encoding == RGB8 || encoding == BGR8 || + encoding == RGBA8 || encoding == BGRA8 || + encoding == RGB16 || encoding == BGR16 || + encoding == RGBA16 || encoding == BGRA16; + } + + static inline bool isMono(const std::string& encoding) + { + return encoding == MONO8 || encoding == MONO16; + } + + static inline bool isBayer(const std::string& encoding) + { + return encoding == BAYER_RGGB8 || encoding == BAYER_BGGR8 || + encoding == BAYER_GBRG8 || encoding == BAYER_GRBG8 || + encoding == BAYER_RGGB16 || encoding == BAYER_BGGR16 || + encoding == BAYER_GBRG16 || encoding == BAYER_GRBG16; + } + + static inline bool hasAlpha(const std::string& encoding) + { + return encoding == RGBA8 || encoding == BGRA8 || + encoding == RGBA16 || encoding == BGRA16; + } + + static inline int numChannels(const std::string& encoding) + { + // First do the common-case encodings + if (encoding == MONO8 || + encoding == MONO16) + return 1; + if (encoding == BGR8 || + encoding == RGB8 || + encoding == BGR16 || + encoding == RGB16) + return 3; + if (encoding == BGRA8 || + encoding == RGBA8 || + encoding == BGRA16 || + encoding == RGBA16) + return 4; + if (encoding == BAYER_RGGB8 || + encoding == BAYER_BGGR8 || + encoding == BAYER_GBRG8 || + encoding == BAYER_GRBG8 || + encoding == BAYER_RGGB16 || + encoding == BAYER_BGGR16 || + encoding == BAYER_GBRG16 || + encoding == BAYER_GRBG16) + return 1; + + // Now all the generic content encodings + // TODO: Rewrite with regex when ROS supports C++11 + for (size_t i=0; i < sizeof(ABSTRACT_ENCODING_PREFIXES) / sizeof(*ABSTRACT_ENCODING_PREFIXES); i++) + { + std::string prefix = ABSTRACT_ENCODING_PREFIXES[i]; + if (encoding.substr(0, prefix.size()) != prefix) + continue; + if (encoding.size() == prefix.size()) + return 1; // ex. 8UC -> 1 + int n_channel = atoi(encoding.substr(prefix.size(), + encoding.size() - prefix.size()).c_str()); // ex. 8UC5 -> 5 + if (n_channel != 0) + return n_channel; // valid encoding string + } + + if (encoding == YUV422) + return 2; + + throw std::runtime_error("Unknown encoding " + encoding); + return -1; + } + + static inline int bitDepth(const std::string& encoding) + { + if (encoding == MONO16) + return 16; + if (encoding == MONO8 || + encoding == BGR8 || + encoding == RGB8 || + encoding == BGRA8 || + encoding == RGBA8 || + encoding == BAYER_RGGB8 || + encoding == BAYER_BGGR8 || + encoding == BAYER_GBRG8 || + encoding == BAYER_GRBG8) + return 8; + + if (encoding == MONO16 || + encoding == BGR16 || + encoding == RGB16 || + encoding == BGRA16 || + encoding == RGBA16 || + encoding == BAYER_RGGB16 || + encoding == BAYER_BGGR16 || + encoding == BAYER_GBRG16 || + encoding == BAYER_GRBG16) + return 16; + + // Now all the generic content encodings + // TODO: Rewrite with regex when ROS supports C++11 + for (size_t i=0; i < sizeof(ABSTRACT_ENCODING_PREFIXES) / sizeof(*ABSTRACT_ENCODING_PREFIXES); i++) + { + std::string prefix = ABSTRACT_ENCODING_PREFIXES[i]; + if (encoding.substr(0, prefix.size()) != prefix) + continue; + if (encoding.size() == prefix.size()) + return atoi(prefix.c_str()); // ex. 8UC -> 8 + int n_channel = atoi(encoding.substr(prefix.size(), + encoding.size() - prefix.size()).c_str()); // ex. 8UC10 -> 10 + if (n_channel != 0) + return atoi(prefix.c_str()); // valid encoding string + } + + if (encoding == YUV422) + return 8; + + throw std::runtime_error("Unknown encoding " + encoding); + return -1; + } + } +} + +#endif diff --git a/fpsdk_common/rosnoros/std_msgs/ByteMultiArray.h b/fpsdk_common/rosnoros/std_msgs/ByteMultiArray.h new file mode 100644 index 00000000..0049629c --- /dev/null +++ b/fpsdk_common/rosnoros/std_msgs/ByteMultiArray.h @@ -0,0 +1,250 @@ +// Generated by gencpp from file std_msgs/ByteMultiArray.msg +// DO NOT EDIT! + + +#ifndef STD_MSGS_MESSAGE_BYTEMULTIARRAY_H +#define STD_MSGS_MESSAGE_BYTEMULTIARRAY_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace std_msgs +{ +template +struct ByteMultiArray_ +{ + typedef ByteMultiArray_ Type; + + ByteMultiArray_() + : layout() + , data() { + } + ByteMultiArray_(const ContainerAllocator& _alloc) + : layout(_alloc) + , data(_alloc) { + (void)_alloc; + } + + + + typedef ::std_msgs::MultiArrayLayout_ _layout_type; + _layout_type layout; + + typedef std::vector::template rebind_alloc> _data_type; + _data_type data; + + + + + + typedef boost::shared_ptr< ::std_msgs::ByteMultiArray_ > Ptr; + typedef boost::shared_ptr< ::std_msgs::ByteMultiArray_ const> ConstPtr; + +}; // struct ByteMultiArray_ + +typedef ::std_msgs::ByteMultiArray_ > ByteMultiArray; + +typedef boost::shared_ptr< ::std_msgs::ByteMultiArray > ByteMultiArrayPtr; +typedef boost::shared_ptr< ::std_msgs::ByteMultiArray const> ByteMultiArrayConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::std_msgs::ByteMultiArray_ & v) +{ +ros::message_operations::Printer< ::std_msgs::ByteMultiArray_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::std_msgs::ByteMultiArray_ & lhs, const ::std_msgs::ByteMultiArray_ & rhs) +{ + return lhs.layout == rhs.layout && + lhs.data == rhs.data; +} + +template +bool operator!=(const ::std_msgs::ByteMultiArray_ & lhs, const ::std_msgs::ByteMultiArray_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace std_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::std_msgs::ByteMultiArray_ > + : TrueType + { }; + +template +struct IsMessage< ::std_msgs::ByteMultiArray_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::std_msgs::ByteMultiArray_ > + : FalseType + { }; + +template +struct IsFixedSize< ::std_msgs::ByteMultiArray_ const> + : FalseType + { }; + +template +struct HasHeader< ::std_msgs::ByteMultiArray_ > + : FalseType + { }; + +template +struct HasHeader< ::std_msgs::ByteMultiArray_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::std_msgs::ByteMultiArray_ > +{ + static const char* value() + { + return "70ea476cbcfd65ac2f68f3cda1e891fe"; + } + + static const char* value(const ::std_msgs::ByteMultiArray_&) { return value(); } + static const uint64_t static_value1 = 0x70ea476cbcfd65acULL; + static const uint64_t static_value2 = 0x2f68f3cda1e891feULL; +}; + +template +struct DataType< ::std_msgs::ByteMultiArray_ > +{ + static const char* value() + { + return "std_msgs/ByteMultiArray"; + } + + static const char* value(const ::std_msgs::ByteMultiArray_&) { return value(); } +}; + +template +struct Definition< ::std_msgs::ByteMultiArray_ > +{ + static const char* value() + { + return "# Please look at the MultiArrayLayout message definition for\n" +"# documentation on all multiarrays.\n" +"\n" +"MultiArrayLayout layout # specification of data layout\n" +"byte[] data # array of data\n" +"\n" +"\n" +"================================================================================\n" +"MSG: std_msgs/MultiArrayLayout\n" +"# The multiarray declares a generic multi-dimensional array of a\n" +"# particular data type. Dimensions are ordered from outer most\n" +"# to inner most.\n" +"\n" +"MultiArrayDimension[] dim # Array of dimension properties\n" +"uint32 data_offset # padding elements at front of data\n" +"\n" +"# Accessors should ALWAYS be written in terms of dimension stride\n" +"# and specified outer-most dimension first.\n" +"# \n" +"# multiarray(i,j,k) = data[data_offset + dim_stride[1]*i + dim_stride[2]*j + k]\n" +"#\n" +"# A standard, 3-channel 640x480 image with interleaved color channels\n" +"# would be specified as:\n" +"#\n" +"# dim[0].label = \"height\"\n" +"# dim[0].size = 480\n" +"# dim[0].stride = 3*640*480 = 921600 (note dim[0] stride is just size of image)\n" +"# dim[1].label = \"width\"\n" +"# dim[1].size = 640\n" +"# dim[1].stride = 3*640 = 1920\n" +"# dim[2].label = \"channel\"\n" +"# dim[2].size = 3\n" +"# dim[2].stride = 3\n" +"#\n" +"# multiarray(i,j,k) refers to the ith row, jth column, and kth channel.\n" +"\n" +"================================================================================\n" +"MSG: std_msgs/MultiArrayDimension\n" +"string label # label of given dimension\n" +"uint32 size # size of given dimension (in type units)\n" +"uint32 stride # stride of given dimension\n" +; + } + + static const char* value(const ::std_msgs::ByteMultiArray_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::std_msgs::ByteMultiArray_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.layout); + stream.next(m.data); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct ByteMultiArray_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::std_msgs::ByteMultiArray_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::std_msgs::ByteMultiArray_& v) + { + s << indent << "layout: "; + s << std::endl; + Printer< ::std_msgs::MultiArrayLayout_ >::stream(s, indent + " ", v.layout); + s << indent << "data[]" << std::endl; + for (size_t i = 0; i < v.data.size(); ++i) + { + s << indent << " data[" << i << "]: "; + Printer::stream(s, indent + " ", v.data[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // STD_MSGS_MESSAGE_BYTEMULTIARRAY_H diff --git a/fpsdk_common/rosnoros/std_msgs/Header.h b/fpsdk_common/rosnoros/std_msgs/Header.h new file mode 100644 index 00000000..4c012028 --- /dev/null +++ b/fpsdk_common/rosnoros/std_msgs/Header.h @@ -0,0 +1,225 @@ +// Generated by gencpp from file std_msgs/Header.msg +// DO NOT EDIT! + + +#ifndef STD_MSGS_MESSAGE_HEADER_H +#define STD_MSGS_MESSAGE_HEADER_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace std_msgs +{ +template +struct Header_ +{ + typedef Header_ Type; + + Header_() + : seq(0) + , stamp() + , frame_id() { + } + Header_(const ContainerAllocator& _alloc) + : seq(0) + , stamp() + , frame_id(_alloc) { + (void)_alloc; + } + + + + typedef uint32_t _seq_type; + _seq_type seq; + + typedef ros::Time _stamp_type; + _stamp_type stamp; + + typedef std::basic_string, typename std::allocator_traits::template rebind_alloc> _frame_id_type; + _frame_id_type frame_id; + + + + + + typedef boost::shared_ptr< ::std_msgs::Header_ > Ptr; + typedef boost::shared_ptr< ::std_msgs::Header_ const> ConstPtr; + +}; // struct Header_ + +typedef ::std_msgs::Header_ > Header; + +typedef boost::shared_ptr< ::std_msgs::Header > HeaderPtr; +typedef boost::shared_ptr< ::std_msgs::Header const> HeaderConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::std_msgs::Header_ & v) +{ +ros::message_operations::Printer< ::std_msgs::Header_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::std_msgs::Header_ & lhs, const ::std_msgs::Header_ & rhs) +{ + return lhs.seq == rhs.seq && + lhs.stamp == rhs.stamp && + lhs.frame_id == rhs.frame_id; +} + +template +bool operator!=(const ::std_msgs::Header_ & lhs, const ::std_msgs::Header_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace std_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::std_msgs::Header_ > + : TrueType + { }; + +template +struct IsMessage< ::std_msgs::Header_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::std_msgs::Header_ > + : FalseType + { }; + +template +struct IsFixedSize< ::std_msgs::Header_ const> + : FalseType + { }; + +template +struct HasHeader< ::std_msgs::Header_ > + : FalseType + { }; + +template +struct HasHeader< ::std_msgs::Header_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::std_msgs::Header_ > +{ + static const char* value() + { + return "2176decaecbce78abc3b96ef049fabed"; + } + + static const char* value(const ::std_msgs::Header_&) { return value(); } + static const uint64_t static_value1 = 0x2176decaecbce78aULL; + static const uint64_t static_value2 = 0xbc3b96ef049fabedULL; +}; + +template +struct DataType< ::std_msgs::Header_ > +{ + static const char* value() + { + return "std_msgs/Header"; + } + + static const char* value(const ::std_msgs::Header_&) { return value(); } +}; + +template +struct Definition< ::std_msgs::Header_ > +{ + static const char* value() + { + return "# Standard metadata for higher-level stamped data types.\n" +"# This is generally used to communicate timestamped data \n" +"# in a particular coordinate frame.\n" +"# \n" +"# sequence ID: consecutively increasing ID \n" +"uint32 seq\n" +"#Two-integer timestamp that is expressed as:\n" +"# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n" +"# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n" +"# time-handling sugar is provided by the client library\n" +"time stamp\n" +"#Frame this data is associated with\n" +"string frame_id\n" +; + } + + static const char* value(const ::std_msgs::Header_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::std_msgs::Header_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.seq); + stream.next(m.stamp); + stream.next(m.frame_id); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct Header_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::std_msgs::Header_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::std_msgs::Header_& v) + { + s << indent << "seq: "; + Printer::stream(s, indent + " ", v.seq); + s << indent << "stamp: "; + Printer::stream(s, indent + " ", v.stamp); + s << indent << "frame_id: "; + Printer, typename std::allocator_traits::template rebind_alloc>>::stream(s, indent + " ", v.frame_id); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // STD_MSGS_MESSAGE_HEADER_H diff --git a/fpsdk_common/rosnoros/std_msgs/MultiArrayDimension.h b/fpsdk_common/rosnoros/std_msgs/MultiArrayDimension.h new file mode 100644 index 00000000..4e64af86 --- /dev/null +++ b/fpsdk_common/rosnoros/std_msgs/MultiArrayDimension.h @@ -0,0 +1,215 @@ +// Generated by gencpp from file std_msgs/MultiArrayDimension.msg +// DO NOT EDIT! + + +#ifndef STD_MSGS_MESSAGE_MULTIARRAYDIMENSION_H +#define STD_MSGS_MESSAGE_MULTIARRAYDIMENSION_H + + +#include +#include +#include + +#include +#include +#include +#include + + +namespace std_msgs +{ +template +struct MultiArrayDimension_ +{ + typedef MultiArrayDimension_ Type; + + MultiArrayDimension_() + : label() + , size(0) + , stride(0) { + } + MultiArrayDimension_(const ContainerAllocator& _alloc) + : label(_alloc) + , size(0) + , stride(0) { + (void)_alloc; + } + + + + typedef std::basic_string, typename std::allocator_traits::template rebind_alloc> _label_type; + _label_type label; + + typedef uint32_t _size_type; + _size_type size; + + typedef uint32_t _stride_type; + _stride_type stride; + + + + + + typedef boost::shared_ptr< ::std_msgs::MultiArrayDimension_ > Ptr; + typedef boost::shared_ptr< ::std_msgs::MultiArrayDimension_ const> ConstPtr; + +}; // struct MultiArrayDimension_ + +typedef ::std_msgs::MultiArrayDimension_ > MultiArrayDimension; + +typedef boost::shared_ptr< ::std_msgs::MultiArrayDimension > MultiArrayDimensionPtr; +typedef boost::shared_ptr< ::std_msgs::MultiArrayDimension const> MultiArrayDimensionConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::std_msgs::MultiArrayDimension_ & v) +{ +ros::message_operations::Printer< ::std_msgs::MultiArrayDimension_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::std_msgs::MultiArrayDimension_ & lhs, const ::std_msgs::MultiArrayDimension_ & rhs) +{ + return lhs.label == rhs.label && + lhs.size == rhs.size && + lhs.stride == rhs.stride; +} + +template +bool operator!=(const ::std_msgs::MultiArrayDimension_ & lhs, const ::std_msgs::MultiArrayDimension_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace std_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::std_msgs::MultiArrayDimension_ > + : TrueType + { }; + +template +struct IsMessage< ::std_msgs::MultiArrayDimension_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::std_msgs::MultiArrayDimension_ > + : FalseType + { }; + +template +struct IsFixedSize< ::std_msgs::MultiArrayDimension_ const> + : FalseType + { }; + +template +struct HasHeader< ::std_msgs::MultiArrayDimension_ > + : FalseType + { }; + +template +struct HasHeader< ::std_msgs::MultiArrayDimension_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::std_msgs::MultiArrayDimension_ > +{ + static const char* value() + { + return "4cd0c83a8683deae40ecdac60e53bfa8"; + } + + static const char* value(const ::std_msgs::MultiArrayDimension_&) { return value(); } + static const uint64_t static_value1 = 0x4cd0c83a8683deaeULL; + static const uint64_t static_value2 = 0x40ecdac60e53bfa8ULL; +}; + +template +struct DataType< ::std_msgs::MultiArrayDimension_ > +{ + static const char* value() + { + return "std_msgs/MultiArrayDimension"; + } + + static const char* value(const ::std_msgs::MultiArrayDimension_&) { return value(); } +}; + +template +struct Definition< ::std_msgs::MultiArrayDimension_ > +{ + static const char* value() + { + return "string label # label of given dimension\n" +"uint32 size # size of given dimension (in type units)\n" +"uint32 stride # stride of given dimension\n" +; + } + + static const char* value(const ::std_msgs::MultiArrayDimension_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::std_msgs::MultiArrayDimension_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.label); + stream.next(m.size); + stream.next(m.stride); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct MultiArrayDimension_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::std_msgs::MultiArrayDimension_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::std_msgs::MultiArrayDimension_& v) + { + s << indent << "label: "; + Printer, typename std::allocator_traits::template rebind_alloc>>::stream(s, indent + " ", v.label); + s << indent << "size: "; + Printer::stream(s, indent + " ", v.size); + s << indent << "stride: "; + Printer::stream(s, indent + " ", v.stride); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // STD_MSGS_MESSAGE_MULTIARRAYDIMENSION_H diff --git a/fpsdk_common/rosnoros/std_msgs/MultiArrayLayout.h b/fpsdk_common/rosnoros/std_msgs/MultiArrayLayout.h new file mode 100644 index 00000000..3b8cd351 --- /dev/null +++ b/fpsdk_common/rosnoros/std_msgs/MultiArrayLayout.h @@ -0,0 +1,242 @@ +// Generated by gencpp from file std_msgs/MultiArrayLayout.msg +// DO NOT EDIT! + + +#ifndef STD_MSGS_MESSAGE_MULTIARRAYLAYOUT_H +#define STD_MSGS_MESSAGE_MULTIARRAYLAYOUT_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace std_msgs +{ +template +struct MultiArrayLayout_ +{ + typedef MultiArrayLayout_ Type; + + MultiArrayLayout_() + : dim() + , data_offset(0) { + } + MultiArrayLayout_(const ContainerAllocator& _alloc) + : dim(_alloc) + , data_offset(0) { + (void)_alloc; + } + + + + typedef std::vector< ::std_msgs::MultiArrayDimension_ , typename std::allocator_traits::template rebind_alloc< ::std_msgs::MultiArrayDimension_ >> _dim_type; + _dim_type dim; + + typedef uint32_t _data_offset_type; + _data_offset_type data_offset; + + + + + + typedef boost::shared_ptr< ::std_msgs::MultiArrayLayout_ > Ptr; + typedef boost::shared_ptr< ::std_msgs::MultiArrayLayout_ const> ConstPtr; + +}; // struct MultiArrayLayout_ + +typedef ::std_msgs::MultiArrayLayout_ > MultiArrayLayout; + +typedef boost::shared_ptr< ::std_msgs::MultiArrayLayout > MultiArrayLayoutPtr; +typedef boost::shared_ptr< ::std_msgs::MultiArrayLayout const> MultiArrayLayoutConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::std_msgs::MultiArrayLayout_ & v) +{ +ros::message_operations::Printer< ::std_msgs::MultiArrayLayout_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::std_msgs::MultiArrayLayout_ & lhs, const ::std_msgs::MultiArrayLayout_ & rhs) +{ + return lhs.dim == rhs.dim && + lhs.data_offset == rhs.data_offset; +} + +template +bool operator!=(const ::std_msgs::MultiArrayLayout_ & lhs, const ::std_msgs::MultiArrayLayout_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace std_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::std_msgs::MultiArrayLayout_ > + : TrueType + { }; + +template +struct IsMessage< ::std_msgs::MultiArrayLayout_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::std_msgs::MultiArrayLayout_ > + : FalseType + { }; + +template +struct IsFixedSize< ::std_msgs::MultiArrayLayout_ const> + : FalseType + { }; + +template +struct HasHeader< ::std_msgs::MultiArrayLayout_ > + : FalseType + { }; + +template +struct HasHeader< ::std_msgs::MultiArrayLayout_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::std_msgs::MultiArrayLayout_ > +{ + static const char* value() + { + return "0fed2a11c13e11c5571b4e2a995a91a3"; + } + + static const char* value(const ::std_msgs::MultiArrayLayout_&) { return value(); } + static const uint64_t static_value1 = 0x0fed2a11c13e11c5ULL; + static const uint64_t static_value2 = 0x571b4e2a995a91a3ULL; +}; + +template +struct DataType< ::std_msgs::MultiArrayLayout_ > +{ + static const char* value() + { + return "std_msgs/MultiArrayLayout"; + } + + static const char* value(const ::std_msgs::MultiArrayLayout_&) { return value(); } +}; + +template +struct Definition< ::std_msgs::MultiArrayLayout_ > +{ + static const char* value() + { + return "# The multiarray declares a generic multi-dimensional array of a\n" +"# particular data type. Dimensions are ordered from outer most\n" +"# to inner most.\n" +"\n" +"MultiArrayDimension[] dim # Array of dimension properties\n" +"uint32 data_offset # padding elements at front of data\n" +"\n" +"# Accessors should ALWAYS be written in terms of dimension stride\n" +"# and specified outer-most dimension first.\n" +"# \n" +"# multiarray(i,j,k) = data[data_offset + dim_stride[1]*i + dim_stride[2]*j + k]\n" +"#\n" +"# A standard, 3-channel 640x480 image with interleaved color channels\n" +"# would be specified as:\n" +"#\n" +"# dim[0].label = \"height\"\n" +"# dim[0].size = 480\n" +"# dim[0].stride = 3*640*480 = 921600 (note dim[0] stride is just size of image)\n" +"# dim[1].label = \"width\"\n" +"# dim[1].size = 640\n" +"# dim[1].stride = 3*640 = 1920\n" +"# dim[2].label = \"channel\"\n" +"# dim[2].size = 3\n" +"# dim[2].stride = 3\n" +"#\n" +"# multiarray(i,j,k) refers to the ith row, jth column, and kth channel.\n" +"\n" +"================================================================================\n" +"MSG: std_msgs/MultiArrayDimension\n" +"string label # label of given dimension\n" +"uint32 size # size of given dimension (in type units)\n" +"uint32 stride # stride of given dimension\n" +; + } + + static const char* value(const ::std_msgs::MultiArrayLayout_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::std_msgs::MultiArrayLayout_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.dim); + stream.next(m.data_offset); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct MultiArrayLayout_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::std_msgs::MultiArrayLayout_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::std_msgs::MultiArrayLayout_& v) + { + s << indent << "dim[]" << std::endl; + for (size_t i = 0; i < v.dim.size(); ++i) + { + s << indent << " dim[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::std_msgs::MultiArrayDimension_ >::stream(s, indent + " ", v.dim[i]); + } + s << indent << "data_offset: "; + Printer::stream(s, indent + " ", v.data_offset); + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // STD_MSGS_MESSAGE_MULTIARRAYLAYOUT_H diff --git a/fpsdk_common/rosnoros/tf2_msgs/TFMessage.h b/fpsdk_common/rosnoros/tf2_msgs/TFMessage.h new file mode 100644 index 00000000..c80acc12 --- /dev/null +++ b/fpsdk_common/rosnoros/tf2_msgs/TFMessage.h @@ -0,0 +1,259 @@ +// Generated by gencpp from file tf2_msgs/TFMessage.msg +// DO NOT EDIT! + + +#ifndef TF2_MSGS_MESSAGE_TFMESSAGE_H +#define TF2_MSGS_MESSAGE_TFMESSAGE_H + + +#include +#include +#include + +#include +#include +#include +#include + +#include + +namespace tf2_msgs +{ +template +struct TFMessage_ +{ + typedef TFMessage_ Type; + + TFMessage_() + : transforms() { + } + TFMessage_(const ContainerAllocator& _alloc) + : transforms(_alloc) { + (void)_alloc; + } + + + + typedef std::vector< ::geometry_msgs::TransformStamped_ , typename std::allocator_traits::template rebind_alloc< ::geometry_msgs::TransformStamped_ >> _transforms_type; + _transforms_type transforms; + + + + + + typedef boost::shared_ptr< ::tf2_msgs::TFMessage_ > Ptr; + typedef boost::shared_ptr< ::tf2_msgs::TFMessage_ const> ConstPtr; + +}; // struct TFMessage_ + +typedef ::tf2_msgs::TFMessage_ > TFMessage; + +typedef boost::shared_ptr< ::tf2_msgs::TFMessage > TFMessagePtr; +typedef boost::shared_ptr< ::tf2_msgs::TFMessage const> TFMessageConstPtr; + +// constants requiring out of line definition + + + +template +std::ostream& operator<<(std::ostream& s, const ::tf2_msgs::TFMessage_ & v) +{ +ros::message_operations::Printer< ::tf2_msgs::TFMessage_ >::stream(s, "", v); +return s; +} + + +template +bool operator==(const ::tf2_msgs::TFMessage_ & lhs, const ::tf2_msgs::TFMessage_ & rhs) +{ + return lhs.transforms == rhs.transforms; +} + +template +bool operator!=(const ::tf2_msgs::TFMessage_ & lhs, const ::tf2_msgs::TFMessage_ & rhs) +{ + return !(lhs == rhs); +} + + +} // namespace tf2_msgs + +namespace ros +{ +namespace message_traits +{ + + + + + +template +struct IsMessage< ::tf2_msgs::TFMessage_ > + : TrueType + { }; + +template +struct IsMessage< ::tf2_msgs::TFMessage_ const> + : TrueType + { }; + +template +struct IsFixedSize< ::tf2_msgs::TFMessage_ > + : FalseType + { }; + +template +struct IsFixedSize< ::tf2_msgs::TFMessage_ const> + : FalseType + { }; + +template +struct HasHeader< ::tf2_msgs::TFMessage_ > + : FalseType + { }; + +template +struct HasHeader< ::tf2_msgs::TFMessage_ const> + : FalseType + { }; + + +template +struct MD5Sum< ::tf2_msgs::TFMessage_ > +{ + static const char* value() + { + return "94810edda583a504dfda3829e70d7eec"; + } + + static const char* value(const ::tf2_msgs::TFMessage_&) { return value(); } + static const uint64_t static_value1 = 0x94810edda583a504ULL; + static const uint64_t static_value2 = 0xdfda3829e70d7eecULL; +}; + +template +struct DataType< ::tf2_msgs::TFMessage_ > +{ + static const char* value() + { + return "tf2_msgs/TFMessage"; + } + + static const char* value(const ::tf2_msgs::TFMessage_&) { return value(); } +}; + +template +struct Definition< ::tf2_msgs::TFMessage_ > +{ + static const char* value() + { + return "geometry_msgs/TransformStamped[] transforms\n" +"\n" +"================================================================================\n" +"MSG: geometry_msgs/TransformStamped\n" +"# This expresses a transform from coordinate frame header.frame_id\n" +"# to the coordinate frame child_frame_id\n" +"#\n" +"# This message is mostly used by the \n" +"# tf package. \n" +"# See its documentation for more information.\n" +"\n" +"Header header\n" +"string child_frame_id # the frame id of the child frame\n" +"Transform transform\n" +"\n" +"================================================================================\n" +"MSG: std_msgs/Header\n" +"# Standard metadata for higher-level stamped data types.\n" +"# This is generally used to communicate timestamped data \n" +"# in a particular coordinate frame.\n" +"# \n" +"# sequence ID: consecutively increasing ID \n" +"uint32 seq\n" +"#Two-integer timestamp that is expressed as:\n" +"# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')\n" +"# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')\n" +"# time-handling sugar is provided by the client library\n" +"time stamp\n" +"#Frame this data is associated with\n" +"string frame_id\n" +"\n" +"================================================================================\n" +"MSG: geometry_msgs/Transform\n" +"# This represents the transform between two coordinate frames in free space.\n" +"\n" +"Vector3 translation\n" +"Quaternion rotation\n" +"\n" +"================================================================================\n" +"MSG: geometry_msgs/Vector3\n" +"# This represents a vector in free space. \n" +"# It is only meant to represent a direction. Therefore, it does not\n" +"# make sense to apply a translation to it (e.g., when applying a \n" +"# generic rigid transformation to a Vector3, tf2 will only apply the\n" +"# rotation). If you want your data to be translatable too, use the\n" +"# geometry_msgs/Point message instead.\n" +"\n" +"float64 x\n" +"float64 y\n" +"float64 z\n" +"================================================================================\n" +"MSG: geometry_msgs/Quaternion\n" +"# This represents an orientation in free space in quaternion form.\n" +"\n" +"float64 x\n" +"float64 y\n" +"float64 z\n" +"float64 w\n" +; + } + + static const char* value(const ::tf2_msgs::TFMessage_&) { return value(); } +}; + +} // namespace message_traits +} // namespace ros + +namespace ros +{ +namespace serialization +{ + + template struct Serializer< ::tf2_msgs::TFMessage_ > + { + template inline static void allInOne(Stream& stream, T m) + { + stream.next(m.transforms); + } + + ROS_DECLARE_ALLINONE_SERIALIZER + }; // struct TFMessage_ + +} // namespace serialization +} // namespace ros + +namespace ros +{ +namespace message_operations +{ + +template +struct Printer< ::tf2_msgs::TFMessage_ > +{ + template static void stream(Stream& s, const std::string& indent, const ::tf2_msgs::TFMessage_& v) + { + s << indent << "transforms[]" << std::endl; + for (size_t i = 0; i < v.transforms.size(); ++i) + { + s << indent << " transforms[" << i << "]: "; + s << std::endl; + s << indent; + Printer< ::geometry_msgs::TransformStamped_ >::stream(s, indent + " ", v.transforms[i]); + } + } +}; + +} // namespace message_operations +} // namespace ros + +#endif // TF2_MSGS_MESSAGE_TFMESSAGE_H diff --git a/fpsdk_common/src/app.cpp b/fpsdk_common/src/app.cpp index fa5dfca0..111df313 100644 --- a/fpsdk_common/src/app.cpp +++ b/fpsdk_common/src/app.cpp @@ -301,11 +301,18 @@ bool ProgramOptions::CheckOptions(const std::vector& /*args*/) void ProgramOptions::PrintVersion() { - std::fprintf(stdout, "%s (%s, %s)\n%s\n%s\n", app_name_.c_str(), + std::fprintf(stdout, "%s (%s, %s, %s)\n%s\n%s\n", app_name_.c_str(), #ifdef NDEBUG "release", #else "debug", +#endif +#if defined(FPSDK_USE_ROS1) + "ROS1", +#elif defined(FPSDK_USE_ROS2) + "ROS2", +#else + "no ROS", #endif utils::GetVersionString(), utils::GetCopyrightString(), utils::GetLicenseString()); } diff --git a/fpsdk_common/src/fpl.cpp b/fpsdk_common/src/fpl.cpp index 3093706a..4a233ce0 100644 --- a/fpsdk_common/src/fpl.cpp +++ b/fpsdk_common/src/fpl.cpp @@ -8,7 +8,7 @@ * \endverbatim * * @file - * @brief Fixposition SDK: .fpl logfile helpers + * @brief Fixposition SDK: .fpl utilities */ /* LIBC/STL */ diff --git a/fpsdk_common/src/parser/fpa.cpp b/fpsdk_common/src/parser/fpa.cpp index 5b90a198..b91b444a 100644 --- a/fpsdk_common/src/parser/fpa.cpp +++ b/fpsdk_common/src/parser/fpa.cpp @@ -124,6 +124,271 @@ bool FpaGetMessageInfo(char* info, const std::size_t size, const uint8_t* msg, c // --------------------------------------------------------------------------------------------------------------------- +const char* FpaInitStatusStr(const FpaInitStatus status) +{ + switch (status) { // clang-format off + case FpaInitStatus::UNSPECIFIED: return "UNSPECIFIED"; + case FpaInitStatus::NOT_INIT: return "NOT_INIT"; + case FpaInitStatus::LOCAL_INIT: return "LOCAL_INIT"; + case FpaInitStatus::GLOBAL_INIT: return "GLOBAL_INIT"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* FpaFusionStatusLegacyStr(const FpaFusionStatusLegacy status) +{ + switch (status) { // clang-format off + case FpaFusionStatusLegacy::UNSPECIFIED: return "UNSPECIFIED"; + case FpaFusionStatusLegacy::NONE: return "NONE"; + case FpaFusionStatusLegacy::VISION: return "VISION"; + case FpaFusionStatusLegacy::VIO: return "VIO"; + case FpaFusionStatusLegacy::IMU_GNSS: return "IMU_GNSS"; + case FpaFusionStatusLegacy::VIO_GNSS: return "VIO_GNSS"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* FpaMeasStatusStr(const FpaMeasStatus status) +{ + switch (status) { // clang-format off + case FpaMeasStatus::UNSPECIFIED: return "UNSPECIFIED"; + case FpaMeasStatus::NOT_USED: return "NOT_USED"; + case FpaMeasStatus::USED: return "USED"; + case FpaMeasStatus::DEGRADED: return "DEGRADED"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* FpaImuStatusStr(const FpaImuStatus status) +{ + switch (status) { // clang-format off + case FpaImuStatus::UNSPECIFIED: return "UNSPECIFIED"; + case FpaImuStatus::NOT_CONVERGED: return "NOT_CONVERGED"; + case FpaImuStatus::WARMSTARTED: return "WARMSTARTED"; + case FpaImuStatus::ROUGH_CONVERGED: return "ROUGH_CONVERGED"; + case FpaImuStatus::FINE_CONVERGED: return "FINE_CONVERGED"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* FpaImuStatusLegacyStr(const FpaImuStatusLegacy status) +{ + switch (status) { // clang-format off + case FpaImuStatusLegacy::UNSPECIFIED: return "UNSPECIFIED"; + case FpaImuStatusLegacy::NOT_CONVERGED: return "NOT_CONVERGED"; + case FpaImuStatusLegacy::CONVERGED: return "CONVERGED"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* FpaImuNoiseStr(const FpaImuNoise noise) +{ + switch (noise) { // clang-format off + case FpaImuNoise::UNSPECIFIED: return "UNSPECIFIED"; + case FpaImuNoise::LOW_NOISE: return "LOW_NOISE"; + case FpaImuNoise::MEDIUM_NOISE: return "MEDIUM_NOISE"; + case FpaImuNoise::HIGH_NOISE: return "HIGH_NOISE"; + case FpaImuNoise::RESERVED4: return "RESERVED4"; + case FpaImuNoise::RESERVED5: return "RESERVED5"; + case FpaImuNoise::RESERVED6: return "RESERVED6"; + case FpaImuNoise::RESERVED7: return "RESERVED7"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* FpaImuConvStr(const FpaImuConv conv) +{ + switch (conv) { // clang-format off + case FpaImuConv::UNSPECIFIED: return "UNSPECIFIED"; + case FpaImuConv::RESERVED0: return "RESERVED0"; + case FpaImuConv::WAIT_IMU_MEAS: return "WAIT_IMU_MEAS"; + case FpaImuConv::WAIT_GLOBAL_MEAS: return "WAIT_GLOBAL_MEAS"; + case FpaImuConv::WAIT_MOTION: return "WAIT_MOTION"; + case FpaImuConv::CONVERGING: return "CONVERGING"; + case FpaImuConv::RESERVED5: return "RESERVED5"; + case FpaImuConv::RESERVED6: return "RESERVED6"; + case FpaImuConv::IDLE: return "IDLE"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* FpaGnssStatusStr(const FpaGnssStatus status) +{ + switch (status) { // clang-format off + case FpaGnssStatus::UNSPECIFIED: return "UNSPECIFIED"; + case FpaGnssStatus::NO_FIX: return "NO_FIX"; + case FpaGnssStatus::SPP: return "SPP"; + case FpaGnssStatus::RTK_MB: return "RTK_MB"; + case FpaGnssStatus::RESERVED3: return "RESERVED3"; + case FpaGnssStatus::RESERVED4: return "RESERVED4"; + case FpaGnssStatus::RTK_FLOAT: return "RTK_FLOAT"; + case FpaGnssStatus::RESERVED6: return "RESERVED6"; + case FpaGnssStatus::RESERVED7: return "RESERVED7"; + case FpaGnssStatus::RTK_FIXED: return "RTK_FIXED"; + case FpaGnssStatus::RESERVED9: return "RESERVED9"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* FpaCorrStatusStr(const FpaCorrStatus status) +{ + switch (status) { // clang-format off + case FpaCorrStatus::UNSPECIFIED: return "UNSPECIFIED"; + case FpaCorrStatus::WAITING_FUSION: return "WAITING_FUSION"; + case FpaCorrStatus::NO_GNSS: return "NO_GNSS"; + case FpaCorrStatus::NO_CORR: return "NO_CORR"; + case FpaCorrStatus::LIMITED_CORR: return "LIMITED_CORR"; + case FpaCorrStatus::OLD_CORR: return "OLD_CORR"; + case FpaCorrStatus::GOOD_CORR: return "GOOD_CORR"; + case FpaCorrStatus::RESERVED6: return "RESERVED6"; + case FpaCorrStatus::RESERVED7: return "RESERVED7"; + case FpaCorrStatus::RESERVED8: return "RESERVED8"; + case FpaCorrStatus::RESERVED9: return "RESERVED9"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* FpaBaselineStatusStr(const FpaBaselineStatus status) +{ + switch (status) { // clang-format off + case FpaBaselineStatus::UNSPECIFIED: return "UNSPECIFIED"; + case FpaBaselineStatus::WAITING_FUSION: return "WAITING_FUSION"; + case FpaBaselineStatus::NO_FIX: return "NO_FIX"; + case FpaBaselineStatus::FAILING: return "FAILING"; + case FpaBaselineStatus::PASSING: return "PASSING"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* FpaCamStatusStr(const FpaCamStatus status) +{ + switch (status) { // clang-format off + case FpaCamStatus::UNSPECIFIED: return "UNSPECIFIED"; + case FpaCamStatus::CAM_UNAVL: return "CAM_UNAVL"; + case FpaCamStatus::BAD_FEAT: return "BAD_FEAT"; + case FpaCamStatus::RESERVED2: return "RESERVED2"; + case FpaCamStatus::RESERVED3: return "RESERVED3"; + case FpaCamStatus::RESERVED4: return "RESERVED4"; + case FpaCamStatus::GOOD: return "GOOD"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* FpaWsStatusStr(const FpaWsStatus status) +{ + switch (status) { // clang-format off + case FpaWsStatus::UNSPECIFIED: return "UNSPECIFIED"; + case FpaWsStatus::NOT_ENABLED: return "NOT_ENABLED"; + case FpaWsStatus::MISS_MEAS: return "MISS_MEAS"; + case FpaWsStatus::NONE_CONVERGED: return "NONE_CONVERGED"; + case FpaWsStatus::ONE_CONVERGED: return "ONE_CONVERGED"; + case FpaWsStatus::ALL_CONVERGED: return "ALL_CONVERGED"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* FpaWsStatusLegacyStr(const FpaWsStatusLegacy status) +{ + switch (status) { // clang-format off + case FpaWsStatusLegacy::UNSPECIFIED: return "UNSPECIFIED"; + case FpaWsStatusLegacy::NOT_ENABLED: return "NOT_ENABLED"; + case FpaWsStatusLegacy::NONE_CONVERGED: return "NONE_CONVERGED"; + case FpaWsStatusLegacy::ONE_OR_MORE_CONVERGED: return "ONE_OR_MORE_CONVERGED"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* FpaWsConvStr(const FpaWsConv status) +{ + switch (status) { // clang-format off + case FpaWsConv::UNSPECIFIED: return "UNSPECIFIED"; + case FpaWsConv::WAIT_FUSION: return "WAIT_FUSION"; + case FpaWsConv::WAIT_WS_MEAS: return "WAIT_WS_MEAS"; + case FpaWsConv::WAIT_GLOBAL_MEAS: return "WAIT_GLOBAL_MEAS"; + case FpaWsConv::WAIT_MOTION: return "WAIT_MOTION"; + case FpaWsConv::WAIT_IMU_BIAS: return "WAIT_IMU_BIAS"; + case FpaWsConv::CONVERGING: return "CONVERGING"; + case FpaWsConv::IDLE: return "IDLE"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* FpaMarkersStatusStr(const FpaMarkersStatus status) +{ + switch (status) { // clang-format off + case FpaMarkersStatus::UNSPECIFIED: return "UNSPECIFIED"; + case FpaMarkersStatus::NOT_ENABLED: return "NOT_ENABLED"; + case FpaMarkersStatus::NONE_CONVERGED: return "NONE_CONVERGED"; + case FpaMarkersStatus::ONE_CONVERGED: return "ONE_CONVERGED"; + case FpaMarkersStatus::ALL_CONVERGED: return "ALL_CONVERGED"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* FpaMarkersConvStr(const FpaMarkersConv conv) +{ + switch (conv) { // clang-format off + case FpaMarkersConv::UNSPECIFIED: return "UNSPECIFIED"; + case FpaMarkersConv::WAIT_FUSION: return "WAIT_FUSION"; + case FpaMarkersConv::WAIT_MARKER_MEAS: return "WAIT_MARKER_MEAS"; + case FpaMarkersConv::WAIT_GLOBAL_MEAS: return "WAIT_GLOBAL_MEAS"; + case FpaMarkersConv::CONVERGING: return "CONVERGING"; + case FpaMarkersConv::IDLE: return "IDLE"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* FpaGnssFixStr(const FpaGnssFix fix) +{ + switch (fix) { // clang-format off + case FpaGnssFix::UNSPECIFIED: return "UNSPECIFIED"; + case FpaGnssFix::UNKNOWN: return "UNKNOWN"; + case FpaGnssFix::NOFIX: return "NOFIX"; + case FpaGnssFix::DRONLY: return "DRONLY"; + case FpaGnssFix::TIME: return "TIME"; + case FpaGnssFix::S2D: return "S2D"; + case FpaGnssFix::S3D: return "S3D"; + case FpaGnssFix::S3D_DR: return "S3D_DR"; + case FpaGnssFix::RTK_FLOAT: return "RTK_FLOAT"; + case FpaGnssFix::RTK_FIXED: return "RTK_FIXED"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + const char* FpaEpochStr(const FpaEpoch epoch) { switch (epoch) { // clang-format off @@ -136,6 +401,82 @@ const char* FpaEpochStr(const FpaEpoch epoch) return "?"; } +// --------------------------------------------------------------------------------------------------------------------- + +const char* FpaAntStateStr(const FpaAntState state) +{ + switch (state) { // clang-format off + case FpaAntState::UNSPECIFIED: return "UNSPECIFIED"; + case FpaAntState::OPEN: return "OPEN"; + case FpaAntState::OK: return "OK"; + case FpaAntState::SHORT: return "SHORT"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* FpaAntPowerStr(const FpaAntPower power) +{ + switch (power) { // clang-format off + case FpaAntPower::UNSPECIFIED: return "UNSPECIFIED"; + case FpaAntPower::ON: return "ON"; + case FpaAntPower::OFF: return "OFF"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* FpaTextLevelStr(const FpaTextLevel level) +{ + switch (level) { // clang-format off + case FpaTextLevel::UNSPECIFIED: return "UNSPECIFIED"; + case FpaTextLevel::ERROR: return "ERROR"; + case FpaTextLevel::WARNING: return "WARNING"; + case FpaTextLevel::INFO: return "INFO"; + case FpaTextLevel::DEBUG: return "DEBUG"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* FpaTimebaseStr(const FpaTimebase base) +{ + switch (base) { // clang-format off + case FpaTimebase::UNSPECIFIED: return "UNSPECIFIED"; + case FpaTimebase::NONE: return "NONE"; + case FpaTimebase::GNSS: return "GNSS"; + case FpaTimebase::UTC: return "UTC"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* FpaTimerefStr(const FpaTimeref ref) +{ + switch (ref) { // clang-format off + case FpaTimeref::UNSPECIFIED: return "UNSPECIFIED"; + case FpaTimeref::UTC_NONE: return "UTC_NONE"; + case FpaTimeref::UTC_CRL: return "UTC_CRL"; + case FpaTimeref::UTC_NIST: return "UTC_NIST"; + case FpaTimeref::UTC_USNO: return "UTC_USNO"; + case FpaTimeref::UTC_BIPM: return "UTC_BIPM"; + case FpaTimeref::UTC_EU: return "UTC_EU"; + case FpaTimeref::UTC_SU: return "UTC_SU"; + case FpaTimeref::UTC_NTSC: return "UTC_NTSC"; + case FpaTimeref::GNSS_GPS: return "GNSS_GPS"; + case FpaTimeref::GNSS_GAL: return "GNSS_GAL"; + case FpaTimeref::GNSS_BDS: return "GNSS_BDS"; + case FpaTimeref::GNSS_GLO: return "GNSS_GLO"; + case FpaTimeref::GNSS_NVC: return "GNSS_NVC"; + case FpaTimeref::OTHER: return "OTHER"; + } // clang-format on + return "?"; +} + // --------------------------------------------------------------------------------------------------------------------- // Various helpers for the SetFromMsg() FP_A decoding functions @@ -932,9 +1273,6 @@ bool FpaRawimuPayload::SetFromMsg(const uint8_t* msg, const std::size_t msg_size bias_comp = (bias.value == 1); } } - if (ok) { - which = Which::RAWIMU; - } FPA_TRACE("FpaRawimuPayload %s", string::ToStr(ok)); valid_ = ok; msg_type_ = FpaMessageType::RAWIMU; @@ -965,9 +1303,6 @@ bool FpaCorrimuPayload::SetFromMsg(const uint8_t* msg, const std::size_t msg_siz bias_comp = (bias.value == 1); } } - if (ok) { - which = Which::CORRIMU; - } FPA_TRACE("FpaCorrimuPayload %s", string::ToStr(ok)); valid_ = ok; msg_type_ = FpaMessageType::CORRIMU; @@ -1033,9 +1368,6 @@ bool FpaOdometryPayload::SetFromMsg(const uint8_t* msg, const std::size_t msg_si GetFloatArr(pos_cov, m.fields_, 23, false) && GetFloatArr(orientation_cov, m.fields_, 29, false) && GetFloatArr(vel_cov, m.fields_, 35, false) && GetText(version, sizeof(version), m.fields_[41])); } - if (ok) { - which = Which::ODOMETRY; - } FPA_TRACE("FpaOdometryPayload %s", string::ToStr(ok)); valid_ = ok; msg_type_ = FpaMessageType::ODOMETRY; @@ -1058,9 +1390,6 @@ bool FpaOdomenuPayload::SetFromMsg(const uint8_t* msg, const std::size_t msg_siz GetFloatArr(pos_cov, m.fields_, 23, false) && GetFloatArr(orientation_cov, m.fields_, 29, false) && GetFloatArr(vel_cov, m.fields_, 35, false)); } - if (ok) { - which = Which::ODOMENU; - } FPA_TRACE("FpaOdomenuPayload %s", string::ToStr(ok)); valid_ = ok; msg_type_ = FpaMessageType::ODOMENU; @@ -1083,9 +1412,6 @@ bool FpaOdomshPayload::SetFromMsg(const uint8_t* msg, const std::size_t msg_size GetFloatArr(pos_cov, m.fields_, 23, false) && GetFloatArr(orientation_cov, m.fields_, 29, false) && GetFloatArr(vel_cov, m.fields_, 35, false)); } - if (ok) { - which = Which::ODOMSH; - } FPA_TRACE("FpaOdomshPayload %s", string::ToStr(ok)); valid_ = ok; msg_type_ = FpaMessageType::ODOMSH; diff --git a/fpsdk_common/src/parser/nmea.cpp b/fpsdk_common/src/parser/nmea.cpp index 01b8c0ac..e480354c 100644 --- a/fpsdk_common/src/parser/nmea.cpp +++ b/fpsdk_common/src/parser/nmea.cpp @@ -216,6 +216,199 @@ NmeaCoordinates::NmeaCoordinates(const double degs, const int digits) // --------------------------------------------------------------------------------------------------------------------- +const char* NmeaTalkerIdStr(const NmeaTalkerId talker) +{ + switch (talker) { // clang-format off + case NmeaTalkerId::UNSPECIFIED: return "UNSPECIFIED"; + case NmeaTalkerId::PROPRIETARY: return "PROPRIETARY"; + case NmeaTalkerId::GPS_SBAS: return "GPS_SBAS"; + case NmeaTalkerId::GLO: return "GLO"; + case NmeaTalkerId::GAL: return "GAL"; + case NmeaTalkerId::BDS: return "BDS"; + case NmeaTalkerId::NAVIC: return "NAVIC"; + case NmeaTalkerId::QZSS: return "QZSS"; + case NmeaTalkerId::GNSS: return "GNSS"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* NmeaFormatterStr(const NmeaFormatter formatter) +{ + switch (formatter) { // clang-format off + case NmeaFormatter::UNSPECIFIED: return "UNSPECIFIED"; + case NmeaFormatter::GGA: return "GGA"; + case NmeaFormatter::GLL: return "GLL"; + case NmeaFormatter::RMC: return "RMC"; + case NmeaFormatter::VTG: return "VTG"; + case NmeaFormatter::GST: return "GST"; + case NmeaFormatter::HDT: return "HDT"; + case NmeaFormatter::ZDA: return "ZDA"; + case NmeaFormatter::GSA: return "GSA"; + case NmeaFormatter::GSV: return "GSV"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* NmeaQualityGgaStr(const NmeaQualityGga qual) +{ + switch (qual) { // clang-format off + case NmeaQualityGga::UNSPECIFIED: return "UNSPECIFIED"; + case NmeaQualityGga::NOFIX: return "NOFIX"; + case NmeaQualityGga::SPP: return "SPP"; + case NmeaQualityGga::DGNSS: return "DGNSS"; + case NmeaQualityGga::PPS: return "PPS"; + case NmeaQualityGga::RTK_FIXED: return "RTK_FIXED"; + case NmeaQualityGga::RTK_FLOAT: return "RTK_FLOAT"; + case NmeaQualityGga::ESTIMATED: return "ESTIMATED"; + case NmeaQualityGga::MANUAL: return "MANUAL"; + case NmeaQualityGga::SIM: return "SIM"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* NmeaStatusGllRmcStr(const NmeaStatusGllRmc status) +{ + switch (status) { // clang-format off + case NmeaStatusGllRmc::UNSPECIFIED: return "UNSPECIFIED"; + case NmeaStatusGllRmc::INVALID: return "INVALID"; + case NmeaStatusGllRmc::VALID: return "VALID"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* NmeaModeGllVtgStr(const NmeaModeGllVtg mode) +{ + switch (mode) { // clang-format off + case NmeaModeGllVtg::UNSPECIFIED: return "UNSPECIFIED"; + case NmeaModeGllVtg::INVALID: return "INVALID"; + case NmeaModeGllVtg::AUTONOMOUS: return "AUTONOMOUS"; + case NmeaModeGllVtg::DGNSS: return "DGNSS"; + case NmeaModeGllVtg::ESTIMATED: return "ESTIMATED"; + case NmeaModeGllVtg::MANUAL: return "MANUAL"; + case NmeaModeGllVtg::SIM: return "SIM"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* NmeaModeRmcGnsStr(const NmeaModeRmcGns mode) +{ + switch (mode) { // clang-format off + case NmeaModeRmcGns::UNSPECIFIED: return "UNSPECIFIED"; + case NmeaModeRmcGns::INVALID: return "INVALID"; + case NmeaModeRmcGns::AUTONOMOUS: return "AUTONOMOUS"; + case NmeaModeRmcGns::DGNSS: return "DGNSS"; + case NmeaModeRmcGns::ESTIMATED: return "ESTIMATED"; + case NmeaModeRmcGns::RTK_FIXED: return "RTK_FIXED"; + case NmeaModeRmcGns::RTK_FLOAT: return "RTK_FLOAT"; + case NmeaModeRmcGns::PRECISE: return "PRECISE"; + case NmeaModeRmcGns::MANUAL: return "MANUAL"; + case NmeaModeRmcGns::SIM: return "SIM"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* NmeaNavStatusRmcStr(const NmeaNavStatusRmc navstatus) +{ + switch (navstatus) { // clang-format off + case NmeaNavStatusRmc::UNSPECIFIED: return "UNSPECIFIED"; + case NmeaNavStatusRmc::SAFE: return "SAFE"; + case NmeaNavStatusRmc::CAUTION: return "CAUTION"; + case NmeaNavStatusRmc::UNSAFE: return "UNSAFE"; + case NmeaNavStatusRmc::NA: return "NA"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* NmeaOpModeGsaStr(const NmeaOpModeGsa opmode) +{ + switch (opmode) { // clang-format off + case NmeaOpModeGsa::UNSPECIFIED: return "UNSPECIFIED"; + case NmeaOpModeGsa::MANUAL: return "MANUAL"; + case NmeaOpModeGsa::AUTO: return "AUTO"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* NmeaNavModeGsaStr(const NmeaNavModeGsa navmode) +{ + switch (navmode) { // clang-format off + case NmeaNavModeGsa::UNSPECIFIED: return "UNSPECIFIED"; + case NmeaNavModeGsa::NOFIX: return "NOFIX"; + case NmeaNavModeGsa::FIX2D: return "FIX2D"; + case NmeaNavModeGsa::FIX3D: return "FIX3D"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* NmeaSystemIdStr(const NmeaSystemId system) +{ + switch (system) { // clang-format off + case NmeaSystemId::UNSPECIFIED: return "UNSPECIFIED"; + case NmeaSystemId::GPS_SBAS: return "GPS_SBAS"; + case NmeaSystemId::GLO: return "GLO"; + case NmeaSystemId::GAL: return "GAL"; + case NmeaSystemId::BDS: return "BDS"; + case NmeaSystemId::QZSS: return "QZSS"; + case NmeaSystemId::NAVIC: return "NAVIC"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + +const char* NmeaSignalIdStr(const NmeaSignalId signal) +{ + switch (signal) { // clang-format off + case NmeaSignalId::UNSPECIFIED: return "UNSPECIFIED"; + case NmeaSignalId::NONE: return "NONE"; + case NmeaSignalId::GPS_L1CA: return "GPS_L1CA"; + case NmeaSignalId::GPS_L2CL: return "GPS_L2CL"; + case NmeaSignalId::GPS_L2CM: return "GPS_L2CM"; + case NmeaSignalId::GPS_L5I: return "GPS_L5I"; + case NmeaSignalId::GPS_L5Q: return "GPS_L5Q"; + case NmeaSignalId::GAL_E1: return "GAL_E1"; + case NmeaSignalId::GAL_E5A: return "GAL_E5A"; + case NmeaSignalId::GAL_E5B: return "GAL_E5B"; + case NmeaSignalId::GAL_E6BC: return "GAL_E6BC"; + case NmeaSignalId::GAL_E6A: return "GAL_E6A"; + case NmeaSignalId::BDS_B1ID: return "BDS_B1ID"; + case NmeaSignalId::BDS_B2ID: return "BDS_B2ID"; + case NmeaSignalId::BDS_B1C: return "BDS_B1C"; + case NmeaSignalId::BDS_B2A: return "BDS_B2A"; + case NmeaSignalId::BDS_B2B: return "BDS_B2B"; + case NmeaSignalId::QZSS_L1CA: return "QZSS_L1CA"; + case NmeaSignalId::QZSS_L1S: return "QZSS_L1S"; + case NmeaSignalId::QZSS_L2CM: return "QZSS_L2CM"; + case NmeaSignalId::QZSS_L2CL: return "QZSS_L2CL"; + case NmeaSignalId::QZSS_L5I: return "QZSS_L5I"; + case NmeaSignalId::QZSS_L5Q: return "QZSS_L5Q"; + case NmeaSignalId::GLO_L1OF: return "GLO_L1OF"; + case NmeaSignalId::GLO_L2OF: return "GLO_L2OF"; + case NmeaSignalId::NAVIC_L5A: return "NAVIC_L5A"; + } // clang-format on + return "?"; +} + +// --------------------------------------------------------------------------------------------------------------------- + // clang-format off // GPS SBAS GAL BDS GLO QZSS NavIC /*static*/ const NmeaVersion NmeaVersion::V410 = { 1, 32, 33, 64, 1, 36, 1, 63, 65, 96, -1, -1, -1, -1 }; diff --git a/fpsdk_common/src/path.cpp b/fpsdk_common/src/path.cpp index 202ef8b1..2adeb93a 100644 --- a/fpsdk_common/src/path.cpp +++ b/fpsdk_common/src/path.cpp @@ -14,6 +14,7 @@ /* LIBC/STL */ #include #include +#include #include /* EXTERNAL */ @@ -93,6 +94,41 @@ std::size_t FileSize(const std::string& path) return size; } +// --------------------------------------------------------------------------------------------------------------------- + +static std::size_t DirSizeEx(const std::string& path, std::size_t depth) +{ + if (depth > 100) { + WARNING("DirSize() max depth exceeded"); + return 0; + } + std::size_t size = 0; + for (const std::filesystem::directory_entry& entry : std::filesystem::directory_iterator(path)) { + if (entry.is_directory()) { + size += DirSizeEx(entry.path(), depth + 1); + } else { + size += entry.file_size(); + } + } + return size; +} + +std::size_t DirSize(const std::string& path) +{ + return DirSizeEx(path, 0); +} + +// --------------------------------------------------------------------------------------------------------------------- + +void RemoveAll(const std::string& path) +{ + std::error_code ec; + std::filesystem::remove_all(path, ec); + if (ec) { + WARNING("Failed to remove %s: %s", path.c_str(), ec.message().c_str()); + } +} + /* ****************************************************************************************************************** */ OutputFile::OutputFile() @@ -163,6 +199,11 @@ bool OutputFile::Write(const uint8_t* data, const std::size_t size) return ok; } +bool OutputFile::Write(const std::string& data) +{ + return Write((const uint8_t*)data.data(), data.size()); +} + // --------------------------------------------------------------------------------------------------------------------- const std::string& OutputFile::Path() const diff --git a/fpsdk_common/src/ros1.cpp b/fpsdk_common/src/ros1.cpp new file mode 100644 index 00000000..5381f7a2 --- /dev/null +++ b/fpsdk_common/src/ros1.cpp @@ -0,0 +1,32 @@ +/** + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ + * \endverbatim + * + * @file + * @brief Fixposition SDK: ROS1 types and utils + */ + +/* LIBC/STL */ + +/* EXTERNAL */ + +/* PACKAGE */ +#include "fpsdk_common/logging.hpp" +#include "fpsdk_common/ros1.hpp" + +namespace fpsdk { +namespace common { +namespace ros1 { +/* ****************************************************************************************************************** */ + +// --------------------------------------------------------------------------------------------------------------------- + +/* ****************************************************************************************************************** */ +} // namespace ros1 +} // namespace common +} // namespace fpsdk diff --git a/fpsdk_common/src/string.cpp b/fpsdk_common/src/string.cpp index 57ad964d..360b7e04 100644 --- a/fpsdk_common/src/string.cpp +++ b/fpsdk_common/src/string.cpp @@ -219,7 +219,8 @@ std::vector HexDump(const uint8_t* data, const std::size_t size) str[pos2] = isprint((int)c) ? c : '.'; } char buf[1024]; - std::snprintf(buf, sizeof(buf), "0x%04" PRIx64 " %05" PRIu64 " %s", ix, ix, str); + std::snprintf(buf, sizeof(buf), + size > 0xffff ? "0x%06" PRIx64 " %08" PRIu64 " %s" : "0x%04" PRIx64 " %05" PRIu64 " %s", ix, ix, str); hexdump.push_back(buf); ix += 16; } diff --git a/fpsdk_common/test/parser_fpa_test.cpp b/fpsdk_common/test/parser_fpa_test.cpp index 8983220b..707f83c7 100644 --- a/fpsdk_common/test/parser_fpa_test.cpp +++ b/fpsdk_common/test/parser_fpa_test.cpp @@ -361,7 +361,6 @@ TEST(ParserFpaTest, FpaRawimuPayload) EXPECT_TRUE(payload.SetFromMsg((const uint8_t*)msg, std::strlen(msg))); EXPECT_TRUE(payload.valid_); EXPECT_EQ(payload.msg_type_, FpaMessageType::RAWIMU); - EXPECT_EQ(payload.which, FpaImuPayload::Which::RAWIMU); EXPECT_TRUE(payload.gps_time.week.valid); EXPECT_EQ(payload.gps_time.week.value, 2197); EXPECT_TRUE(payload.gps_time.tow.valid); @@ -384,7 +383,6 @@ TEST(ParserFpaTest, FpaRawimuPayload) EXPECT_TRUE(payload.SetFromMsg((const uint8_t*)msg, std::strlen(msg))); EXPECT_TRUE(payload.valid_); EXPECT_EQ(payload.msg_type_, FpaMessageType::RAWIMU); - EXPECT_EQ(payload.which, FpaImuPayload::Which::RAWIMU); EXPECT_FALSE(payload.gps_time.week.valid); EXPECT_EQ(payload.gps_time.week.value, 0); EXPECT_FALSE(payload.gps_time.tow.valid); @@ -408,7 +406,6 @@ TEST(ParserFpaTest, FpaRawimuPayload) EXPECT_TRUE(payload.SetFromMsg((const uint8_t*)msg, std::strlen(msg))); EXPECT_TRUE(payload.valid_); EXPECT_EQ(payload.msg_type_, FpaMessageType::RAWIMU); - EXPECT_EQ(payload.which, FpaImuPayload::Which::RAWIMU); EXPECT_TRUE(payload.gps_time.week.valid); EXPECT_EQ(payload.gps_time.week.value, 2368); EXPECT_TRUE(payload.gps_time.tow.valid); @@ -438,7 +435,6 @@ TEST(ParserFpaTest, FpaCorrimuPayload) EXPECT_TRUE(payload.SetFromMsg((const uint8_t*)msg, std::strlen(msg))); EXPECT_TRUE(payload.valid_); EXPECT_EQ(payload.msg_type_, FpaMessageType::CORRIMU); - EXPECT_EQ(payload.which, FpaImuPayload::Which::CORRIMU); EXPECT_TRUE(payload.gps_time.week.valid); EXPECT_EQ(payload.gps_time.week.value, 2197); EXPECT_TRUE(payload.gps_time.tow.valid); @@ -459,7 +455,6 @@ TEST(ParserFpaTest, FpaCorrimuPayload) EXPECT_TRUE(payload.SetFromMsg((const uint8_t*)msg, std::strlen(msg))); EXPECT_TRUE(payload.valid_); EXPECT_EQ(payload.msg_type_, FpaMessageType::CORRIMU); - EXPECT_EQ(payload.which, FpaImuPayload::Which::CORRIMU); EXPECT_FALSE(payload.gps_time.week.valid); EXPECT_EQ(payload.gps_time.week.value, 0); EXPECT_FALSE(payload.gps_time.tow.valid); @@ -481,7 +476,6 @@ TEST(ParserFpaTest, FpaCorrimuPayload) EXPECT_TRUE(payload.SetFromMsg((const uint8_t*)msg, std::strlen(msg))); EXPECT_TRUE(payload.valid_); EXPECT_EQ(payload.msg_type_, FpaMessageType::CORRIMU); - EXPECT_EQ(payload.which, FpaImuPayload::Which::CORRIMU); EXPECT_TRUE(payload.gps_time.week.valid); EXPECT_EQ(payload.gps_time.week.value, 2368); EXPECT_TRUE(payload.gps_time.tow.valid); @@ -633,7 +627,6 @@ TEST(ParserFpaTest, FpaOdometryPayload) EXPECT_TRUE(payload.SetFromMsg((const uint8_t*)msg, std::strlen(msg))); EXPECT_TRUE(payload.valid_); EXPECT_EQ(payload.msg_type_, FpaMessageType::ODOMETRY); - EXPECT_EQ(payload.which, FpaOdomPayload::Which::ODOMETRY); EXPECT_TRUE(payload.gps_time.week.valid); EXPECT_EQ(payload.gps_time.week.value, 2231); EXPECT_TRUE(payload.gps_time.tow.valid); @@ -703,7 +696,6 @@ TEST(ParserFpaTest, FpaOdomenuPayload) EXPECT_TRUE(payload.SetFromMsg((const uint8_t*)msg, std::strlen(msg))); EXPECT_TRUE(payload.valid_); EXPECT_EQ(payload.msg_type_, FpaMessageType::ODOMENU); - EXPECT_EQ(payload.which, FpaOdomPayload::Which::ODOMENU); EXPECT_TRUE(payload.gps_time.week.valid); EXPECT_EQ(payload.gps_time.week.value, 2180); EXPECT_TRUE(payload.gps_time.tow.valid); @@ -726,7 +718,6 @@ TEST(ParserFpaTest, FpaOdomshPayload) EXPECT_TRUE(payload.SetFromMsg((const uint8_t*)msg, std::strlen(msg))); EXPECT_TRUE(payload.valid_); EXPECT_EQ(payload.msg_type_, FpaMessageType::ODOMSH); - EXPECT_EQ(payload.which, FpaOdomPayload::Which::ODOMSH); EXPECT_TRUE(payload.gps_time.week.valid); EXPECT_EQ(payload.gps_time.week.value, 2180); EXPECT_TRUE(payload.gps_time.tow.valid); diff --git a/fpsdk_common/test/ros1_test.cpp b/fpsdk_common/test/ros1_test.cpp new file mode 100644 index 00000000..c2a4f779 --- /dev/null +++ b/fpsdk_common/test/ros1_test.cpp @@ -0,0 +1,46 @@ +/** + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG (www.fixposition.com) and contributors + * / /\ \ License: see the LICENSE file + * /__/ \__\ + * \endverbatim + * + * @file + * @brief Fixposition SDK: tests for fpsdk::common::ros1 + */ + +/* LIBC/STL */ + +/* EXTERNAL */ +#include + +/* PACKAGE */ +#include +#include + +namespace { +/* ****************************************************************************************************************** */ +using namespace fpsdk::common::ros1; + +TEST(Ros1, 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_doc/Doxyfile b/fpsdk_doc/Doxyfile index 46c10633..bd8efd95 100644 --- a/fpsdk_doc/Doxyfile +++ b/fpsdk_doc/Doxyfile @@ -1,7 +1,7 @@ -# Doxyfile 1.11.0 +# Doxyfile 1.14.0 # This file describes the settings to be used by the documentation system -# doxygen (www.doxygen.org) for a project. +# Doxygen (www.doxygen.org) for a project. # # All text after a double hash (##) is considered a comment and is placed in # front of the TAG it is preceding. @@ -15,10 +15,10 @@ # # Note: # -# Use doxygen to compare the used configuration file with the template +# Use Doxygen to compare the used configuration file with the template # configuration file: # doxygen -x [configFile] -# Use doxygen to compare the used configuration file with the template +# Use Doxygen to compare the used configuration file with the template # configuration file without replacing the environment variables or CMake type # replacement variables: # doxygen -x_noenv [configFile] @@ -51,7 +51,7 @@ PROJECT_NAME = "Fixposition SDK" PROJECT_NUMBER = # Using the PROJECT_BRIEF tag one can provide an optional one line description -# for a project that appears at the top of each page and should give viewer a +# for a project that appears at the top of each page and should give viewers a # quick idea about the purpose of the project. Keep the description short. PROJECT_BRIEF = "Collection of c++ libraries and apps for use with Fixposition products on Linux" @@ -71,16 +71,16 @@ PROJECT_ICON = fpsdk_doc/favicon.png # The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path # into which the generated documentation will be written. If a relative path is -# entered, it will be relative to the location where doxygen was started. If +# entered, it will be relative to the location where Doxygen was started. If # left blank the current directory will be used. OUTPUT_DIRECTORY = build -# If the CREATE_SUBDIRS tag is set to YES then doxygen will create up to 4096 +# If the CREATE_SUBDIRS tag is set to YES then Doxygen will create up to 4096 # sub-directories (in 2 levels) under the output directory of each output format # and will distribute the generated files over these directories. Enabling this -# option can be useful when feeding doxygen a huge amount of source files, where -# putting all generated files in the same directory would otherwise causes +# option can be useful when feeding Doxygen a huge amount of source files, where +# putting all generated files in the same directory would otherwise cause # performance problems for the file system. Adapt CREATE_SUBDIRS_LEVEL to # control the number of sub-directories. # The default value is: NO. @@ -98,7 +98,7 @@ CREATE_SUBDIRS = NO CREATE_SUBDIRS_LEVEL = 8 -# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII +# If the ALLOW_UNICODE_NAMES tag is set to YES, Doxygen will allow non-ASCII # characters to appear in the names of generated files. If set to NO, non-ASCII # characters will be escaped, for example _xE3_x81_x84 will be used for Unicode # U+3044. @@ -107,7 +107,7 @@ CREATE_SUBDIRS_LEVEL = 8 ALLOW_UNICODE_NAMES = NO # The OUTPUT_LANGUAGE tag is used to specify the language in which all -# documentation generated by doxygen is written. Doxygen will use this +# documentation generated by Doxygen is written. Doxygen will use this # information to generate all constant output in the proper language. # Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Bulgarian, # Catalan, Chinese, Chinese-Traditional, Croatian, Czech, Danish, Dutch, English @@ -121,14 +121,14 @@ ALLOW_UNICODE_NAMES = NO OUTPUT_LANGUAGE = English -# If the BRIEF_MEMBER_DESC tag is set to YES, doxygen will include brief member +# If the BRIEF_MEMBER_DESC tag is set to YES, Doxygen will include brief member # descriptions after the members that are listed in the file and class # documentation (similar to Javadoc). Set to NO to disable this. # The default value is: YES. BRIEF_MEMBER_DESC = YES -# If the REPEAT_BRIEF tag is set to YES, doxygen will prepend the brief +# If the REPEAT_BRIEF tag is set to YES, Doxygen will prepend the brief # description of a member or function before the detailed description # # Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the @@ -149,13 +149,13 @@ REPEAT_BRIEF = YES ABBREVIATE_BRIEF = # If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then -# doxygen will generate a detailed section even if there is only a brief +# Doxygen will generate a detailed section even if there is only a brief # description. # The default value is: NO. ALWAYS_DETAILED_SEC = NO -# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all +# If the INLINE_INHERITED_MEMB tag is set to YES, Doxygen will show all # inherited members of a class in the documentation of that class as if those # members were ordinary class members. Constructors, destructors and assignment # operators of the base classes will not be shown. @@ -163,7 +163,7 @@ ALWAYS_DETAILED_SEC = NO INLINE_INHERITED_MEMB = NO -# If the FULL_PATH_NAMES tag is set to YES, doxygen will prepend the full path +# If the FULL_PATH_NAMES tag is set to YES, Doxygen will prepend the full path # before files name in the file list and in the header files. If set to NO the # shortest path that makes the file name unique will be used # The default value is: YES. @@ -173,11 +173,11 @@ FULL_PATH_NAMES = YES # The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path. # Stripping is only done if one of the specified strings matches the left-hand # part of the path. The tag can be used to show relative paths in the file list. -# If left blank the directory from which doxygen is run is used as the path to +# If left blank the directory from which Doxygen is run is used as the path to # strip. # # Note that you can specify absolute paths here, but also relative paths, which -# will be relative from the directory where doxygen is started. +# will be relative from the directory where Doxygen is started. # This tag requires that the tag FULL_PATH_NAMES is set to YES. STRIP_FROM_PATH = @@ -191,41 +191,42 @@ STRIP_FROM_PATH = STRIP_FROM_INC_PATH = -# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but -# less readable) file names. This can be useful is your file systems doesn't +# If the SHORT_NAMES tag is set to YES, Doxygen will generate much shorter (but +# less readable) file names. This can be useful if your file system doesn't # support long names like on DOS, Mac, or CD-ROM. # The default value is: NO. SHORT_NAMES = NO -# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the -# first line (until the first dot) of a Javadoc-style comment as the brief -# description. If set to NO, the Javadoc-style will behave just like regular Qt- -# style comments (thus requiring an explicit @brief command for a brief -# description.) +# If the JAVADOC_AUTOBRIEF tag is set to YES then Doxygen will interpret the +# first line (until the first dot, question mark or exclamation mark) of a +# Javadoc-style comment as the brief description. If set to NO, the Javadoc- +# style will behave just like regular Qt-style comments (thus requiring an +# explicit @brief command for a brief description.) # The default value is: NO. JAVADOC_AUTOBRIEF = NO -# If the JAVADOC_BANNER tag is set to YES then doxygen will interpret a line +# If the JAVADOC_BANNER tag is set to YES then Doxygen will interpret a line # such as # /*************** # as being the beginning of a Javadoc-style comment "banner". If set to NO, the # Javadoc-style will behave just like regular comments and it will not be -# interpreted by doxygen. +# interpreted by Doxygen. # The default value is: NO. JAVADOC_BANNER = NO -# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first -# line (until the first dot) of a Qt-style comment as the brief description. If -# set to NO, the Qt-style will behave just like regular Qt-style comments (thus -# requiring an explicit \brief command for a brief description.) +# If the QT_AUTOBRIEF tag is set to YES then Doxygen will interpret the first +# line (until the first dot, question mark or exclamation mark) of a Qt-style +# comment as the brief description. If set to NO, the Qt-style will behave just +# like regular Qt-style comments (thus requiring an explicit \brief command for +# a brief description.) # The default value is: NO. QT_AUTOBRIEF = NO -# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a +# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make Doxygen treat a # multi-line C++ special comment block (i.e. a block of //! or /// comments) as # a brief description. This used to be the default behavior. The new default is # to treat a multi-line C++ comment block as a detailed description. Set this @@ -237,10 +238,10 @@ QT_AUTOBRIEF = NO MULTILINE_CPP_IS_BRIEF = NO -# By default Python docstrings are displayed as preformatted text and doxygen's +# By default Python docstrings are displayed as preformatted text and Doxygen's # special commands cannot be used. By setting PYTHON_DOCSTRING to NO the -# doxygen's special commands can be used and the contents of the docstring -# documentation blocks is shown as doxygen documentation. +# Doxygen's special commands can be used and the contents of the docstring +# documentation blocks is shown as Doxygen documentation. # The default value is: YES. PYTHON_DOCSTRING = YES @@ -251,7 +252,7 @@ PYTHON_DOCSTRING = YES INHERIT_DOCS = YES -# If the SEPARATE_MEMBER_PAGES tag is set to YES then doxygen will produce a new +# If the SEPARATE_MEMBER_PAGES tag is set to YES then Doxygen will produce a new # page for each member. If set to NO, the documentation of a member will be part # of the file/class/namespace that contains it. # The default value is: NO. @@ -279,11 +280,10 @@ TAB_SIZE = 4 # with the commands \{ and \} for these it is advised to use the version @{ and # @} or use a double escape (\\{ and \\}) -ALIASES = \ - fp_msgspec_begin{1}="@anchor msgspec_\1" \ - fp_msgspec_end="" \ - fp_codegen_begin{1}="" \ - fp_codegen_end{1}="" +ALIASES = "fp_msgspec_begin{1}=@anchor msgspec_\1" \ + "fp_msgspec_end=" \ + "fp_codegen_begin{1}=" \ + "fp_codegen_end{1}=" # Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources # only. Doxygen will then generate output that is more tailored for C. For @@ -325,30 +325,30 @@ OPTIMIZE_OUTPUT_SLICE = NO # parses. With this tag you can assign which parser to use for a given # extension. Doxygen has a built-in mapping, but you can override or extend it # using this tag. The format is ext=language, where ext is a file extension, and -# language is one of the parsers supported by doxygen: IDL, Java, JavaScript, +# language is one of the parsers supported by Doxygen: IDL, Java, JavaScript, # Csharp (C#), C, C++, Lex, D, PHP, md (Markdown), Objective-C, Python, Slice, # VHDL, Fortran (fixed format Fortran: FortranFixed, free formatted Fortran: # FortranFree, unknown formatted Fortran: Fortran. In the later case the parser # tries to guess whether the code is fixed or free formatted code, this is the -# default for Fortran type files). For instance to make doxygen treat .inc files +# default for Fortran type files). For instance to make Doxygen treat .inc files # as Fortran files (default is PHP), and .f files as C (default is Fortran), # use: inc=Fortran f=C. # # Note: For files without extension you can use no_extension as a placeholder. # # Note that for custom extensions you also need to set FILE_PATTERNS otherwise -# the files are not read by doxygen. When specifying no_extension you should add +# the files are not read by Doxygen. When specifying no_extension you should add # * to the FILE_PATTERNS. # # Note see also the list of default file extension mappings. EXTENSION_MAPPING = -# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments +# If the MARKDOWN_SUPPORT tag is enabled then Doxygen pre-processes all comments # according to the Markdown format, which allows for more readable # documentation. See https://daringfireball.net/projects/markdown/ for details. -# The output of markdown processing is further processed by doxygen, so you can -# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in +# The output of markdown processing is further processed by Doxygen, so you can +# mix Doxygen, HTML, and XML commands with Markdown formatting. Disable only in # case of backward compatibilities issues. # The default value is: YES. @@ -374,17 +374,26 @@ TOC_INCLUDE_HEADINGS = 5 MARKDOWN_ID_STYLE = DOXYGEN -# When enabled doxygen tries to link words that correspond to documented +# When enabled Doxygen tries to link words that correspond to documented # classes, or namespaces to their corresponding documentation. Such a link can # be prevented in individual cases by putting a % sign in front of the word or -# globally by setting AUTOLINK_SUPPORT to NO. +# globally by setting AUTOLINK_SUPPORT to NO. Words listed in the +# AUTOLINK_IGNORE_WORDS tag are excluded from automatic linking. # The default value is: YES. AUTOLINK_SUPPORT = YES +# This tag specifies a list of words that, when matching the start of a word in +# the documentation, will suppress auto links generation, if it is enabled via +# AUTOLINK_SUPPORT. This list does not affect links explicitly created using \# +# or the \link or commands. +# This tag requires that the tag AUTOLINK_SUPPORT is set to YES. + +AUTOLINK_IGNORE_WORDS = + # If you use STL classes (i.e. std::string, std::vector, etc.) but do not want # to include (a tag file for) the STL sources as input, then you should set this -# tag to YES in order to let doxygen match functions declarations and +# tag to YES in order to let Doxygen match functions declarations and # definitions whose arguments contain STL classes (e.g. func(std::string); # versus func(std::string) {}). This also makes the inheritance and # collaboration diagrams that involve STL classes more complete and accurate. @@ -408,7 +417,7 @@ SIP_SUPPORT = NO # For Microsoft's IDL there are propget and propput attributes to indicate # getter and setter methods for a property. Setting this option to YES will make -# doxygen to replace the get and set methods by a property in the documentation. +# Doxygen to replace the get and set methods by a property in the documentation. # This will only work if the methods are indeed getting or setting a simple # type. If this is not the case, or you want to show the methods anyway, you # should set this option to NO. @@ -417,7 +426,7 @@ SIP_SUPPORT = NO IDL_PROPERTY_SUPPORT = NO # If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC -# tag is set to YES then doxygen will reuse the documentation of the first +# tag is set to YES then Doxygen will reuse the documentation of the first # member in the group (if any) for the other members of the group. By default # all members of a group must be documented explicitly. # The default value is: NO. @@ -475,18 +484,18 @@ TYPEDEF_HIDES_STRUCT = NO # The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This # cache is used to resolve symbols given their name and scope. Since this can be # an expensive process and often the same symbol appears multiple times in the -# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small -# doxygen will become slower. If the cache is too large, memory is wasted. The +# code, Doxygen keeps a cache of pre-resolved symbols. If the cache is too small +# Doxygen will become slower. If the cache is too large, memory is wasted. The # cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range # is 0..9, the default is 0, corresponding to a cache size of 2^16=65536 -# symbols. At the end of a run doxygen will report the cache usage and suggest +# symbols. At the end of a run Doxygen will report the cache usage and suggest # the optimal cache size from a speed point of view. # Minimum value: 0, maximum value: 9, default value: 0. LOOKUP_CACHE_SIZE = 0 -# The NUM_PROC_THREADS specifies the number of threads doxygen is allowed to use -# during processing. When set to 0 doxygen will based this on the number of +# The NUM_PROC_THREADS specifies the number of threads Doxygen is allowed to use +# during processing. When set to 0 Doxygen will based this on the number of # cores available in the system. You can set it explicitly to a value larger # than 0 to get more control over the balance between CPU load and processing # speed. At this moment only the input processing can be done using multiple @@ -510,7 +519,7 @@ TIMESTAMP = YES # Build related configuration options #--------------------------------------------------------------------------- -# If the EXTRACT_ALL tag is set to YES, doxygen will assume all entities in +# If the EXTRACT_ALL tag is set to YES, Doxygen will assume all entities in # documentation are documented, even if no documentation was available. Private # class members and static file members will be hidden unless the # EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES. @@ -576,7 +585,7 @@ EXTRACT_ANON_NSPACES = NO RESOLVE_UNNAMED_PARAMS = YES -# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all +# If the HIDE_UNDOC_MEMBERS tag is set to YES, Doxygen will hide all # undocumented members inside documented classes or files. If set to NO these # members will be included in the various overviews, but no documentation # section is generated. This option has no effect if EXTRACT_ALL is enabled. @@ -584,7 +593,7 @@ RESOLVE_UNNAMED_PARAMS = YES HIDE_UNDOC_MEMBERS = NO -# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all +# If the HIDE_UNDOC_CLASSES tag is set to YES, Doxygen will hide all # undocumented classes that are normally visible in the class hierarchy. If set # to NO, these classes will be included in the various overviews. This option # will also hide undocumented C++ concepts if enabled. This option has no effect @@ -593,14 +602,22 @@ HIDE_UNDOC_MEMBERS = NO HIDE_UNDOC_CLASSES = NO -# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend +# If the HIDE_UNDOC_NAMESPACES tag is set to YES, Doxygen will hide all +# undocumented namespaces that are normally visible in the namespace hierarchy. +# If set to NO, these namespaces will be included in the various overviews. This +# option has no effect if EXTRACT_ALL is enabled. +# The default value is: YES. + +HIDE_UNDOC_NAMESPACES = YES + +# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, Doxygen will hide all friend # declarations. If set to NO, these declarations will be included in the # documentation. # The default value is: NO. HIDE_FRIEND_COMPOUNDS = NO -# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any +# If the HIDE_IN_BODY_DOCS tag is set to YES, Doxygen will hide any # documentation blocks found inside the body of a function. If set to NO, these # blocks will be appended to the function's detailed documentation block. # The default value is: NO. @@ -614,7 +631,7 @@ HIDE_IN_BODY_DOCS = NO INTERNAL_DOCS = NO -# With the correct setting of option CASE_SENSE_NAMES doxygen will better be +# With the correct setting of option CASE_SENSE_NAMES Doxygen will better be # able to match the capabilities of the underlying filesystem. In case the # filesystem is case sensitive (i.e. it supports files in the same directory # whose names only differ in casing), the option must be set to YES to properly @@ -623,7 +640,7 @@ INTERNAL_DOCS = NO # output files written for symbols that only differ in casing, such as for two # classes, one named CLASS and the other named Class, and to also support # references to files without having to specify the exact matching casing. On -# Windows (including Cygwin) and MacOS, users should typically set this option +# Windows (including Cygwin) and macOS, users should typically set this option # to NO, whereas on Linux or other Unix flavors it should typically be set to # YES. # Possible values are: SYSTEM, NO and YES. @@ -631,14 +648,14 @@ INTERNAL_DOCS = NO CASE_SENSE_NAMES = YES -# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with +# If the HIDE_SCOPE_NAMES tag is set to NO then Doxygen will show members with # their full class and namespace scopes in the documentation. If set to YES, the # scope will be hidden. # The default value is: NO. HIDE_SCOPE_NAMES = NO -# If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then doxygen will +# If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then Doxygen will # append additional text to a page's title, such as Class Reference. If set to # YES the compound reference will be hidden. # The default value is: NO. @@ -651,7 +668,7 @@ HIDE_COMPOUND_REFERENCE= NO SHOW_HEADERFILE = YES -# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of +# If the SHOW_INCLUDE_FILES tag is set to YES then Doxygen will put a list of # the files that are included by a file in the documentation of that file. # The default value is: YES. @@ -664,7 +681,7 @@ SHOW_INCLUDE_FILES = YES SHOW_GROUPED_MEMB_INC = NO -# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include +# If the FORCE_LOCAL_INCLUDES tag is set to YES then Doxygen will list include # files with double quotes in the documentation rather than with sharp brackets. # The default value is: NO. @@ -676,14 +693,14 @@ FORCE_LOCAL_INCLUDES = NO INLINE_INFO = YES -# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the +# If the SORT_MEMBER_DOCS tag is set to YES then Doxygen will sort the # (detailed) documentation of file and class members alphabetically by member # name. If set to NO, the members will appear in declaration order. # The default value is: YES. SORT_MEMBER_DOCS = NO -# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief +# If the SORT_BRIEF_DOCS tag is set to YES then Doxygen will sort the brief # descriptions of file, namespace and class members alphabetically by member # name. If set to NO, the members will appear in declaration order. Note that # this will also influence the order of the classes in the class list. @@ -691,7 +708,7 @@ SORT_MEMBER_DOCS = NO SORT_BRIEF_DOCS = NO -# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the +# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then Doxygen will sort the # (brief and detailed) documentation of class members so that constructors and # destructors are listed first. If set to NO the constructors will appear in the # respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS. @@ -703,7 +720,7 @@ SORT_BRIEF_DOCS = NO SORT_MEMBERS_CTORS_1ST = NO -# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy +# If the SORT_GROUP_NAMES tag is set to YES then Doxygen will sort the hierarchy # of group names into alphabetical order. If set to NO the group names will # appear in their defined order. # The default value is: NO. @@ -720,11 +737,11 @@ SORT_GROUP_NAMES = NO SORT_BY_SCOPE_NAME = NO -# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper +# If the STRICT_PROTO_MATCHING option is enabled and Doxygen fails to do proper # type resolution of all parameters of a function it will reject a match between # the prototype and the implementation of a member function even if there is # only one candidate or it is obvious which candidate to choose by doing a -# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still +# simple string match. By disabling STRICT_PROTO_MATCHING Doxygen will still # accept a match between prototype and implementation in such cases. # The default value is: NO. @@ -794,25 +811,25 @@ SHOW_FILES = YES SHOW_NAMESPACES = YES # The FILE_VERSION_FILTER tag can be used to specify a program or script that -# doxygen should invoke to get the current version for each file (typically from +# Doxygen should invoke to get the current version for each file (typically from # the version control system). Doxygen will invoke the program by executing (via # popen()) the command command input-file, where command is the value of the # FILE_VERSION_FILTER tag, and input-file is the name of an input file provided -# by doxygen. Whatever the program writes to standard output is used as the file +# by Doxygen. Whatever the program writes to standard output is used as the file # version. For an example see the documentation. FILE_VERSION_FILTER = # The LAYOUT_FILE tag can be used to specify a layout file which will be parsed -# by doxygen. The layout file controls the global structure of the generated +# by Doxygen. The layout file controls the global structure of the generated # output files in an output format independent way. To create the layout file -# that represents doxygen's defaults, run doxygen with the -l option. You can +# that represents Doxygen's defaults, run Doxygen with the -l option. You can # optionally specify a file name after the option, if omitted DoxygenLayout.xml # will be used as the name of the layout file. See also section "Changing the # layout of pages" for information. # -# Note that if you run doxygen from a directory containing a file called -# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE +# Note that if you run Doxygen from a directory containing a file called +# DoxygenLayout.xml, Doxygen will parse it automatically even if the LAYOUT_FILE # tag is left empty. LAYOUT_FILE = @@ -827,19 +844,35 @@ LAYOUT_FILE = CITE_BIB_FILES = +# The EXTERNAL_TOOL_PATH tag can be used to extend the search path (PATH +# environment variable) so that external tools such as latex and gs can be +# found. +# Note: Directories specified with EXTERNAL_TOOL_PATH are added in front of the +# path already specified by the PATH variable, and are added in the order +# specified. +# Note: This option is particularly useful for macOS version 14 (Sonoma) and +# higher, when running Doxygen from Doxywizard, because in this case any user- +# defined changes to the PATH are ignored. A typical example on macOS is to set +# EXTERNAL_TOOL_PATH = /Library/TeX/texbin /usr/local/bin +# together with the standard path, the full search path used by doxygen when +# launching external tools will then become +# PATH=/Library/TeX/texbin:/usr/local/bin:/usr/bin:/bin:/usr/sbin:/sbin + +EXTERNAL_TOOL_PATH = + #--------------------------------------------------------------------------- # Configuration options related to warning and progress messages #--------------------------------------------------------------------------- # The QUIET tag can be used to turn on/off the messages that are generated to -# standard output by doxygen. If QUIET is set to YES this implies that the +# standard output by Doxygen. If QUIET is set to YES this implies that the # messages are off. # The default value is: NO. QUIET = YES # The WARNINGS tag can be used to turn on/off the warning messages that are -# generated to standard error (stderr) by doxygen. If WARNINGS is set to YES +# generated to standard error (stderr) by Doxygen. If WARNINGS is set to YES # this implies that the warnings are on. # # Tip: Turn warnings on while writing the documentation. @@ -847,14 +880,14 @@ QUIET = YES WARNINGS = YES -# If the WARN_IF_UNDOCUMENTED tag is set to YES then doxygen will generate +# If the WARN_IF_UNDOCUMENTED tag is set to YES then Doxygen will generate # warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag # will automatically be disabled. # The default value is: YES. WARN_IF_UNDOCUMENTED = YES -# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for +# If the WARN_IF_DOC_ERROR tag is set to YES, Doxygen will generate warnings for # potential errors in the documentation, such as documenting some parameters in # a documented function twice, or documenting parameters that don't exist or # using markup commands wrongly. @@ -862,8 +895,8 @@ WARN_IF_UNDOCUMENTED = YES WARN_IF_DOC_ERROR = YES -# If WARN_IF_INCOMPLETE_DOC is set to YES, doxygen will warn about incomplete -# function parameter documentation. If set to NO, doxygen will accept that some +# If WARN_IF_INCOMPLETE_DOC is set to YES, Doxygen will warn about incomplete +# function parameter documentation. If set to NO, Doxygen will accept that some # parameters have no documentation without warning. # The default value is: YES. @@ -871,7 +904,7 @@ WARN_IF_INCOMPLETE_DOC = YES # This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that # are documented, but have no documentation for their parameters or return -# value. If set to NO, doxygen will only warn about wrong parameter +# value. If set to NO, Doxygen will only warn about wrong parameter # documentation, but not about the absence of documentation. If EXTRACT_ALL is # set to YES then this flag will automatically be disabled. See also # WARN_IF_INCOMPLETE_DOC @@ -879,20 +912,28 @@ WARN_IF_INCOMPLETE_DOC = YES WARN_NO_PARAMDOC = NO -# If WARN_IF_UNDOC_ENUM_VAL option is set to YES, doxygen will warn about -# undocumented enumeration values. If set to NO, doxygen will accept +# If WARN_IF_UNDOC_ENUM_VAL option is set to YES, Doxygen will warn about +# undocumented enumeration values. If set to NO, Doxygen will accept # undocumented enumeration values. If EXTRACT_ALL is set to YES then this flag # will automatically be disabled. # The default value is: NO. WARN_IF_UNDOC_ENUM_VAL = YES -# If the WARN_AS_ERROR tag is set to YES then doxygen will immediately stop when +# If WARN_LAYOUT_FILE option is set to YES, Doxygen will warn about issues found +# while parsing the user defined layout file, such as missing or wrong elements. +# See also LAYOUT_FILE for details. If set to NO, problems with the layout file +# will be suppressed. +# The default value is: YES. + +WARN_LAYOUT_FILE = YES + +# If the WARN_AS_ERROR tag is set to YES then Doxygen will immediately stop when # a warning is encountered. If the WARN_AS_ERROR tag is set to FAIL_ON_WARNINGS -# then doxygen will continue running as if WARN_AS_ERROR tag is set to NO, but -# at the end of the doxygen process doxygen will return with a non-zero status. -# If the WARN_AS_ERROR tag is set to FAIL_ON_WARNINGS_PRINT then doxygen behaves -# like FAIL_ON_WARNINGS but in case no WARN_LOGFILE is defined doxygen will not +# then Doxygen will continue running as if WARN_AS_ERROR tag is set to NO, but +# at the end of the Doxygen process Doxygen will return with a non-zero status. +# If the WARN_AS_ERROR tag is set to FAIL_ON_WARNINGS_PRINT then Doxygen behaves +# like FAIL_ON_WARNINGS but in case no WARN_LOGFILE is defined Doxygen will not # write the warning messages in between other messages but write them at the end # of a run, in case a WARN_LOGFILE is defined the warning messages will be # besides being in the defined file also be shown at the end of a run, unless @@ -903,7 +944,7 @@ WARN_IF_UNDOC_ENUM_VAL = YES WARN_AS_ERROR = NO -# The WARN_FORMAT tag determines the format of the warning messages that doxygen +# The WARN_FORMAT tag determines the format of the warning messages that Doxygen # can produce. The string should contain the $file, $line, and $text tags, which # will be replaced by the file and line number from which the warning originated # and the warning text. Optionally the format may contain $version, which will @@ -916,7 +957,7 @@ WARN_FORMAT = "$file:$line: $text" # In the $text part of the WARN_FORMAT command it is possible that a reference # to a more specific place is given. To make it easier to jump to this place -# (outside of doxygen) the user can define a custom "cut" / "paste" string. +# (outside of Doxygen) the user can define a custom "cut" / "paste" string. # Example: # WARN_LINE_FORMAT = "'vi $file +$line'" # See also: WARN_FORMAT @@ -944,8 +985,12 @@ WARN_LOGFILE = # Note: If this tag is empty the current directory is searched. INPUT = fpsdk_doc \ - fpsdk_common/doc fpsdk_ros1/doc fpsdk_ros2/doc \ - fpsdk_common/include fpsdk_ros1/include fpsdk_ros2/include \ + 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 \ @@ -954,7 +999,7 @@ INPUT = fpsdk_doc \ fpsdk_apps/yaml2shell/yaml2shell_doc.hpp # This tag can be used to specify the character encoding of the source files -# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses +# that Doxygen parses. Internally Doxygen uses the UTF-8 encoding. Doxygen uses # libiconv (or the iconv built into libc) for the transcoding. See the libiconv # documentation (see: # https://www.gnu.org/software/libiconv/) for the list of possible encodings. @@ -964,10 +1009,10 @@ INPUT = fpsdk_doc \ INPUT_ENCODING = UTF-8 # This tag can be used to specify the character encoding of the source files -# that doxygen parses The INPUT_FILE_ENCODING tag can be used to specify +# that Doxygen parses. The INPUT_FILE_ENCODING tag can be used to specify # character encoding on a per file pattern basis. Doxygen will compare the file # name with each pattern and apply the encoding instead of the default -# INPUT_ENCODING) if there is a match. The character encodings are a list of the +# INPUT_ENCODING if there is a match. The character encodings are a list of the # form: pattern=encoding (like *.php=ISO-8859-1). # See also: INPUT_ENCODING for further information on supported encodings. @@ -979,16 +1024,16 @@ INPUT_FILE_ENCODING = # # Note that for custom extensions or not directly supported extensions you also # need to set EXTENSION_MAPPING for the extension otherwise the files are not -# read by doxygen. +# read by Doxygen. # # Note the list of default checked file patterns might differ from the list of # default file extension mappings. # # If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cxxm, # *.cpp, *.cppm, *.ccm, *.c++, *.c++m, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, -# *.idl, *.ddl, *.odl, *.h, *.hh, *.hxx, *.hpp, *.h++, *.ixx, *.l, *.cs, *.d, -# *.php, *.php4, *.php5, *.phtml, *.inc, *.m, *.markdown, *.md, *.mm, *.dox (to -# be provided as doxygen C comment), *.py, *.pyw, *.f90, *.f95, *.f03, *.f08, +# *.idl, *.ddl, *.odl, *.h, *.hh, *.hxx, *.hpp, *.h++, *.l, *.cs, *.d, *.php, +# *.php4, *.php5, *.phtml, *.inc, *.m, *.markdown, *.md, *.mm, *.dox (to be +# provided as Doxygen C comment), *.py, *.pyw, *.f90, *.f95, *.f03, *.f08, # *.f18, *.f, *.for, *.vhd, *.vhdl, *.ucf, *.qsf and *.ice. FILE_PATTERNS = *.hpp @@ -1003,10 +1048,10 @@ RECURSIVE = YES # excluded from the INPUT source files. This way you can easily exclude a # subdirectory from a directory tree whose root is specified with the INPUT tag. # -# Note that relative paths are relative to the directory from which doxygen is +# Note that relative paths are relative to the directory from which Doxygen is # run. -EXCLUDE = +EXCLUDE = fpsdk_common/include/fpsdk_common/rosnoros # The EXCLUDE_SYMLINKS tag can be used to select whether or not files or # directories that are symbolic links (a Unix file system feature) are excluded @@ -1037,10 +1082,15 @@ EXCLUDE_SYMBOLS = # command). EXAMPLE_PATH = fpsdk_doc \ - fpsdk_common fpsdk_common/doc \ - fpsdk_ros1 fpsdk_ros1/doc fpsdk_ros1/msg \ - fpsdk_ros2 fpsdk_ros2/doc \ - fpsdk_apps fpsdk_apps/doc \ + fpsdk_common \ + fpsdk_common/doc \ + fpsdk_ros1 \ + fpsdk_ros1/doc \ + fpsdk_ros1/msg \ + fpsdk_ros2 \ + fpsdk_ros2/doc \ + fpsdk_apps \ + fpsdk_apps/doc \ examples/parser_intro \ examples/fusion_epoch \ examples/fpb_measurements @@ -1068,7 +1118,7 @@ IMAGE_PATH = fpsdk_doc \ fpsdk_ros1/doc \ fpsdk_apps/doc -# The INPUT_FILTER tag can be used to specify a program that doxygen should +# 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 # by executing (via popen()) the command: # @@ -1083,14 +1133,14 @@ IMAGE_PATH = fpsdk_doc \ # code is scanned, but not when the output code is generated. If lines are added # or removed, the anchors will not be placed correctly. # -# Note that doxygen will use the data processed and written to standard output +# Note that Doxygen will use the data processed and written to standard output # for further processing, therefore nothing else, like debug statements or used # commands (so in case of a Windows batch file always use @echo OFF), should be # written to standard output. # # Note that for custom extensions or not directly supported extensions you also # need to set EXTENSION_MAPPING for the extension otherwise the files are not -# properly processed by doxygen. +# properly processed by Doxygen. INPUT_FILTER = @@ -1103,7 +1153,7 @@ INPUT_FILTER = # # Note that for custom extensions or not directly supported extensions you also # need to set EXTENSION_MAPPING for the extension otherwise the files are not -# properly processed by doxygen. +# properly processed by Doxygen. FILTER_PATTERNS = @@ -1125,10 +1175,19 @@ FILTER_SOURCE_PATTERNS = # If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that # is part of the input, its contents will be placed on the main page # (index.html). This can be useful if you have a project on for instance GitHub -# and want to reuse the introduction page also for the doxygen output. +# and want to reuse the introduction page also for the Doxygen output. USE_MDFILE_AS_MAINPAGE = +# If the IMPLICIT_DIR_DOCS tag is set to YES, any README.md file found in sub- +# directories of the project's root, is used as the documentation for that sub- +# directory, except when the README.md starts with a \dir, \page or \mainpage +# command. If set to NO, the README.md file needs to start with an explicit \dir +# command in order to be used as directory documentation. +# The default value is: YES. + +IMPLICIT_DIR_DOCS = NO + # The Fortran standard specifies that for fixed formatted Fortran code all # characters from position 72 are to be considered as comment. A common # extension is to allow longer lines before the automatic comment starts. The @@ -1158,7 +1217,7 @@ SOURCE_BROWSER = YES INLINE_SOURCES = NO -# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any +# Setting the STRIP_CODE_COMMENTS tag to YES will instruct Doxygen to hide any # special comment blocks from generated source code fragments. Normal C, C++ and # Fortran comments will always remain visible. # The default value is: YES. @@ -1196,7 +1255,7 @@ REFERENCES_LINK_SOURCE = YES SOURCE_TOOLTIPS = YES # If the USE_HTAGS tag is set to YES then the references to source code will -# point to the HTML generated by the htags(1) tool instead of doxygen built-in +# point to the HTML generated by the htags(1) tool instead of Doxygen built-in # source browser. The htags tool is part of GNU's global source tagging system # (see https://www.gnu.org/software/global/global.html). You will need version # 4.8.6 or higher. @@ -1210,14 +1269,14 @@ SOURCE_TOOLTIPS = YES # Doxygen will invoke htags (and that will in turn invoke gtags), so these # tools must be available from the command line (i.e. in the search path). # -# The result: instead of the source browser generated by doxygen, the links to +# The result: instead of the source browser generated by Doxygen, the links to # source code will now point to the output of htags. # The default value is: NO. # This tag requires that the tag SOURCE_BROWSER is set to YES. USE_HTAGS = NO -# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a +# If the VERBATIM_HEADERS tag is set the YES then Doxygen will generate a # verbatim copy of the header file for each class for which an include is # specified. Set to NO to disable this. # See also: Section \class. @@ -1249,7 +1308,7 @@ IGNORE_PREFIX = # Configuration options related to the HTML output #--------------------------------------------------------------------------- -# If the GENERATE_HTML tag is set to YES, doxygen will generate HTML output +# If the GENERATE_HTML tag is set to YES, Doxygen will generate HTML output # The default value is: YES. GENERATE_HTML = YES @@ -1270,40 +1329,40 @@ HTML_OUTPUT = doc HTML_FILE_EXTENSION = .html # The HTML_HEADER tag can be used to specify a user-defined HTML header file for -# each generated HTML page. If the tag is left blank doxygen will generate a +# each generated HTML page. If the tag is left blank Doxygen will generate a # standard header. # # To get valid HTML the header file that includes any scripts and style sheets -# that doxygen needs, which is dependent on the configuration options used (e.g. +# that Doxygen needs, which is dependent on the configuration options used (e.g. # the setting GENERATE_TREEVIEW). It is highly recommended to start with a # default header using # doxygen -w html new_header.html new_footer.html new_stylesheet.css # YourConfigFile # and then modify the file new_header.html. See also section "Doxygen usage" -# for information on how to generate the default header that doxygen normally +# for information on how to generate the default header that Doxygen normally # uses. # Note: The header is subject to change so you typically have to regenerate the -# default header when upgrading to a newer version of doxygen. For a description +# default header when upgrading to a newer version of Doxygen. For a description # of the possible markers and block names see the documentation. # This tag requires that the tag GENERATE_HTML is set to YES. HTML_HEADER = # The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each -# generated HTML page. If the tag is left blank doxygen will generate a standard +# generated HTML page. If the tag is left blank Doxygen will generate a standard # footer. See HTML_HEADER for more information on how to generate a default # footer and what special commands can be used inside the footer. See also # section "Doxygen usage" for information on how to generate the default footer -# that doxygen normally uses. +# that Doxygen normally uses. # This tag requires that the tag GENERATE_HTML is set to YES. HTML_FOOTER = # The HTML_STYLESHEET tag can be used to specify a user-defined cascading style # sheet that is used by each HTML page. It can be used to fine-tune the look of -# the HTML output. If left blank doxygen will generate a default style sheet. +# the HTML output. If left blank Doxygen will generate a default style sheet. # See also section "Doxygen usage" for information on how to generate the style -# sheet that doxygen normally uses. +# sheet that Doxygen normally uses. # Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as # it is more robust and this tag (HTML_STYLESHEET) will in the future become # obsolete. @@ -1313,7 +1372,7 @@ HTML_STYLESHEET = # The HTML_EXTRA_STYLESHEET tag can be used to specify additional user-defined # cascading style sheets that are included after the standard style sheets -# created by doxygen. Using this option one can overrule certain style aspects. +# created by Doxygen. Using this option one can overrule certain style aspects. # This is preferred over using HTML_STYLESHEET since it does not replace the # standard style sheet and is therefore more robust against future updates. # Doxygen will copy the style sheet files to the output directory. @@ -1408,7 +1467,7 @@ HTML_DYNAMIC_SECTIONS = NO HTML_CODE_FOLDING = NO -# If the HTML_COPY_CLIPBOARD tag is set to YES then doxygen will show an icon in +# If the HTML_COPY_CLIPBOARD tag is set to YES then Doxygen will show an icon in # the top right corner of code and text fragments that allows the user to copy # its content to the clipboard. Note this only works if supported by the browser # and the web page is served via a secure context (see: @@ -1421,7 +1480,7 @@ HTML_COPY_CLIPBOARD = YES # Doxygen stores a couple of settings persistently in the browser (via e.g. # cookies). By default these settings apply to all HTML pages generated by -# doxygen across all projects. The HTML_PROJECT_COOKIE tag can be used to store +# Doxygen across all projects. The HTML_PROJECT_COOKIE tag can be used to store # the settings under a project specific key, such that the user preferences will # be stored separately. # This tag requires that the tag GENERATE_HTML is set to YES. @@ -1445,7 +1504,7 @@ HTML_INDEX_NUM_ENTRIES = 100 # generated that can be used as input for Apple's Xcode 3 integrated development # environment (see: # https://developer.apple.com/xcode/), introduced with OSX 10.5 (Leopard). To -# create a documentation set, doxygen will generate a Makefile in the HTML +# create a documentation set, Doxygen will generate a Makefile in the HTML # output directory. Running make will produce the docset in that directory and # running make install will install the docset in # ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at @@ -1493,18 +1552,18 @@ DOCSET_PUBLISHER_ID = org.doxygen.Publisher DOCSET_PUBLISHER_NAME = Publisher -# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three +# If the GENERATE_HTMLHELP tag is set to YES then Doxygen generates three # additional HTML index files: index.hhp, index.hhc, and index.hhk. The # index.hhp is a project file that can be read by Microsoft's HTML Help Workshop # on Windows. In the beginning of 2021 Microsoft took the original page, with -# a.o. the download links, offline the HTML help workshop was already many years -# in maintenance mode). You can download the HTML help workshop from the web -# archives at Installation executable (see: +# a.o. the download links, offline (the HTML help workshop was already many +# years in maintenance mode). You can download the HTML help workshop from the +# web archives at Installation executable (see: # http://web.archive.org/web/20160201063255/http://download.microsoft.com/downlo # ad/0/A/9/0A939EF6-E31C-430F-A3DF-DFAE7960D564/htmlhelp.exe). # # The HTML Help Workshop contains a compiler that can convert all HTML output -# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML +# generated by Doxygen into a single compiled HTML file (.chm). Compiled HTML # files are now used as the Windows 98 help format, and will replace the old # Windows help format (.hlp) on all Windows platforms in the future. Compressed # HTML files also contain an index, a table of contents, and you can search for @@ -1524,7 +1583,7 @@ CHM_FILE = # The HHC_LOCATION tag can be used to specify the location (absolute path # including file name) of the HTML help compiler (hhc.exe). If non-empty, -# doxygen will try to run the HTML help compiler on the generated index.hhp. +# Doxygen will try to run the HTML help compiler on the generated index.hhp. # The file has to be specified with full path. # This tag requires that the tag GENERATE_HTMLHELP is set to YES. @@ -1626,7 +1685,7 @@ QHP_CUST_FILTER_ATTRS = QHP_SECT_FILTER_ATTRS = # The QHG_LOCATION tag can be used to specify the location (absolute path -# including file name) of Qt's qhelpgenerator. If non-empty doxygen will try to +# including file name) of Qt's qhelpgenerator. If non-empty Doxygen will try to # run qhelpgenerator on the generated .qhp file. # This tag requires that the tag GENERATE_QHP is set to YES. @@ -1671,29 +1730,38 @@ DISABLE_INDEX = NO # (i.e. any modern browser). Windows users are probably better off using the # HTML help feature. Via custom style sheets (see HTML_EXTRA_STYLESHEET) one can # further fine tune the look of the index (see "Fine-tuning the output"). As an -# example, the default style sheet generated by doxygen has an example that +# example, the default style sheet generated by Doxygen has an example that # shows how to put an image at the root of the tree instead of the PROJECT_NAME. -# Since the tree basically has the same information as the tab index, you could -# consider setting DISABLE_INDEX to YES when enabling this option. -# The default value is: NO. +# Since the tree basically has more details information than the tab index, you +# could consider setting DISABLE_INDEX to YES when enabling this option. +# The default value is: YES. # This tag requires that the tag GENERATE_HTML is set to YES. GENERATE_TREEVIEW = YES -# When both GENERATE_TREEVIEW and DISABLE_INDEX are set to YES, then the -# FULL_SIDEBAR option determines if the side bar is limited to only the treeview -# area (value NO) or if it should extend to the full height of the window (value -# YES). Setting this to YES gives a layout similar to -# https://docs.readthedocs.io with more room for contents, but less room for the -# project logo, title, and description. If either GENERATE_TREEVIEW or -# DISABLE_INDEX is set to NO, this option has no effect. +# When GENERATE_TREEVIEW is set to YES, the PAGE_OUTLINE_PANEL option determines +# if an additional navigation panel is shown at the right hand side of the +# screen, displaying an outline of the contents of the main page, similar to +# e.g. https://developer.android.com/reference If GENERATE_TREEVIEW is set to +# NO, this option has no effect. +# The default value is: YES. +# This tag requires that the tag GENERATE_HTML is set to YES. + +PAGE_OUTLINE_PANEL = YES + +# When GENERATE_TREEVIEW is set to YES, the FULL_SIDEBAR option determines if +# the side bar is limited to only the treeview area (value NO) or if it should +# extend to the full height of the window (value YES). Setting this to YES gives +# a layout similar to e.g. https://docs.readthedocs.io with more room for +# contents, but less room for the project logo, title, and description. If +# GENERATE_TREEVIEW is set to NO, this option has no effect. # The default value is: NO. # This tag requires that the tag GENERATE_HTML is set to YES. FULL_SIDEBAR = NO # The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that -# doxygen will group on one line in the generated HTML documentation. +# Doxygen will group on one line in the generated HTML documentation. # # Note that a value of 0 will completely suppress the enum values from appearing # in the overview section. @@ -1702,6 +1770,12 @@ FULL_SIDEBAR = NO ENUM_VALUES_PER_LINE = 4 +# When the SHOW_ENUM_VALUES tag is set doxygen will show the specified +# enumeration values besides the enumeration mnemonics. +# The default value is: NO. + +SHOW_ENUM_VALUES = NO + # If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used # to set the initial width (in pixels) of the frame in which the tree is shown. # Minimum value: 0, maximum value: 1500, default value: 250. @@ -1709,21 +1783,21 @@ ENUM_VALUES_PER_LINE = 4 TREEVIEW_WIDTH = 250 -# If the EXT_LINKS_IN_WINDOW option is set to YES, doxygen will open links to +# If the EXT_LINKS_IN_WINDOW option is set to YES, Doxygen will open links to # external symbols imported via tag files in a separate window. # The default value is: NO. # This tag requires that the tag GENERATE_HTML is set to YES. EXT_LINKS_IN_WINDOW = NO -# If the OBFUSCATE_EMAILS tag is set to YES, doxygen will obfuscate email +# If the OBFUSCATE_EMAILS tag is set to YES, Doxygen will obfuscate email # addresses. # The default value is: YES. # This tag requires that the tag GENERATE_HTML is set to YES. OBFUSCATE_EMAILS = YES -# If the HTML_FORMULA_FORMAT option is set to svg, doxygen will use the pdf2svg +# If the HTML_FORMULA_FORMAT option is set to svg, Doxygen will use the pdf2svg # tool (see https://github.com/dawbarton/pdf2svg) or inkscape (see # https://inkscape.org) to generate formulas as SVG images instead of PNGs for # the HTML output. These images will generally look nicer at scaled resolutions. @@ -1736,7 +1810,7 @@ HTML_FORMULA_FORMAT = png # Use this tag to change the font size of LaTeX formulas included as images in # the HTML documentation. When you change the font size after a successful -# doxygen run you need to manually remove any form_*.png images from the HTML +# Doxygen run you need to manually remove any form_*.png images from the HTML # output directory to force them to be regenerated. # Minimum value: 8, maximum value: 50, default value: 10. # This tag requires that the tag GENERATE_HTML is set to YES. @@ -1814,7 +1888,7 @@ MATHJAX_RELPATH = https://cdn.jsdelivr.net/npm/mathjax@2 MATHJAX_EXTENSIONS = -# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces +# The MATHJAX_CODEFILE tag can be used to specify a file with JavaScript pieces # of code that will be used on startup of the MathJax code. See the MathJax site # (see: # http://docs.mathjax.org/en/v2.7-latest/output.html) for more details. For an @@ -1823,12 +1897,12 @@ MATHJAX_EXTENSIONS = MATHJAX_CODEFILE = -# When the SEARCHENGINE tag is enabled doxygen will generate a search box for -# the HTML output. The underlying search engine uses javascript and DHTML and +# When the SEARCHENGINE tag is enabled Doxygen will generate a search box for +# the HTML output. The underlying search engine uses JavaScript and DHTML and # should work on any modern browser. Note that when using HTML help # (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET) # there is already a search function so this one should typically be disabled. -# For large projects the javascript based search engine can be slow, then +# For large projects the JavaScript based search engine can be slow, then # enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to # search using the keyboard; to jump to the search box use + S # (what the is depends on the OS and browser, but it is typically @@ -1847,7 +1921,7 @@ SEARCHENGINE = YES # When the SERVER_BASED_SEARCH tag is enabled the search engine will be # implemented using a web server instead of a web client using JavaScript. There # are two flavors of web server based searching depending on the EXTERNAL_SEARCH -# setting. When disabled, doxygen will generate a PHP script for searching and +# setting. When disabled, Doxygen will generate a PHP script for searching and # an index file used by the script. When EXTERNAL_SEARCH is enabled the indexing # and searching needs to be provided by external tools. See the section # "External Indexing and Searching" for details. @@ -1856,7 +1930,7 @@ SEARCHENGINE = YES SERVER_BASED_SEARCH = NO -# When EXTERNAL_SEARCH tag is enabled doxygen will no longer generate the PHP +# When EXTERNAL_SEARCH tag is enabled Doxygen will no longer generate the PHP # script for searching. Instead the search results are written to an XML file # which needs to be processed by an external indexer. Doxygen will invoke an # external search engine pointed to by the SEARCHENGINE_URL option to obtain the @@ -1901,7 +1975,7 @@ SEARCHDATA_FILE = searchdata.xml EXTERNAL_SEARCH_ID = -# The EXTRA_SEARCH_MAPPINGS tag can be used to enable searching through doxygen +# The EXTRA_SEARCH_MAPPINGS tag can be used to enable searching through Doxygen # projects other than the one defined by this configuration file, but that are # all added to the same external search index. Each project needs to have a # unique id set via EXTERNAL_SEARCH_ID. The search mapping then maps the id of @@ -1915,7 +1989,7 @@ EXTRA_SEARCH_MAPPINGS = # Configuration options related to the LaTeX output #--------------------------------------------------------------------------- -# If the GENERATE_LATEX tag is set to YES, doxygen will generate LaTeX output. +# If the GENERATE_LATEX tag is set to YES, Doxygen will generate LaTeX output. # The default value is: YES. GENERATE_LATEX = NO @@ -1960,7 +2034,7 @@ MAKEINDEX_CMD_NAME = makeindex LATEX_MAKEINDEX_CMD = makeindex -# If the COMPACT_LATEX tag is set to YES, doxygen generates more compact LaTeX +# If the COMPACT_LATEX tag is set to YES, Doxygen generates more compact LaTeX # documents. This may be useful for small projects and may help to save some # trees in general. # The default value is: NO. @@ -1991,15 +2065,15 @@ EXTRA_PACKAGES = # The LATEX_HEADER tag can be used to specify a user-defined LaTeX header for # the generated LaTeX document. The header should contain everything until the -# first chapter. If it is left blank doxygen will generate a standard header. It +# first chapter. If it is left blank Doxygen will generate a standard header. It # is highly recommended to start with a default header using # doxygen -w latex new_header.tex new_footer.tex new_stylesheet.sty # and then modify the file new_header.tex. See also section "Doxygen usage" for -# information on how to generate the default header that doxygen normally uses. +# information on how to generate the default header that Doxygen normally uses. # # Note: Only use a user-defined header if you know what you are doing! # Note: The header is subject to change so you typically have to regenerate the -# default header when upgrading to a newer version of doxygen. The following +# default header when upgrading to a newer version of Doxygen. The following # commands have a special meaning inside the header (and footer): For a # description of the possible markers and block names see the documentation. # This tag requires that the tag GENERATE_LATEX is set to YES. @@ -2008,10 +2082,10 @@ LATEX_HEADER = # The LATEX_FOOTER tag can be used to specify a user-defined LaTeX footer for # the generated LaTeX document. The footer should contain everything after the -# last chapter. If it is left blank doxygen will generate a standard footer. See +# last chapter. If it is left blank Doxygen will generate a standard footer. See # LATEX_HEADER for more information on how to generate a default footer and what # special commands can be used inside the footer. See also section "Doxygen -# usage" for information on how to generate the default footer that doxygen +# usage" for information on how to generate the default footer that Doxygen # normally uses. Note: Only use a user-defined footer if you know what you are # doing! # This tag requires that the tag GENERATE_LATEX is set to YES. @@ -2020,7 +2094,7 @@ LATEX_FOOTER = # The LATEX_EXTRA_STYLESHEET tag can be used to specify additional user-defined # LaTeX style sheets that are included after the standard style sheets created -# by doxygen. Using this option one can overrule certain style aspects. Doxygen +# by Doxygen. Using this option one can overrule certain style aspects. Doxygen # will copy the style sheet files to the output directory. # Note: The order of the extra style sheet files is of importance (e.g. the last # style sheet in the list overrules the setting of the previous ones in the @@ -2046,7 +2120,7 @@ LATEX_EXTRA_FILES = PDF_HYPERLINKS = YES -# If the USE_PDFLATEX tag is set to YES, doxygen will use the engine as +# If the USE_PDFLATEX tag is set to YES, Doxygen will use the engine as # specified with LATEX_CMD_NAME to generate the PDF file directly from the LaTeX # files. Set this option to YES, to get a higher quality PDF documentation. # @@ -2071,7 +2145,7 @@ USE_PDFLATEX = YES LATEX_BATCHMODE = NO -# If the LATEX_HIDE_INDICES tag is set to YES then doxygen will not include the +# If the LATEX_HIDE_INDICES tag is set to YES then Doxygen will not include the # index chapters (such as File Index, Compound Index, etc.) in the output. # The default value is: NO. # This tag requires that the tag GENERATE_LATEX is set to YES. @@ -2081,7 +2155,7 @@ LATEX_HIDE_INDICES = NO # The LATEX_BIB_STYLE tag can be used to specify the style to use for the # bibliography, e.g. plainnat, or ieeetr. See # https://en.wikipedia.org/wiki/BibTeX and \cite for more info. -# The default value is: plain. +# The default value is: plainnat. # This tag requires that the tag GENERATE_LATEX is set to YES. LATEX_BIB_STYLE = plain @@ -2098,7 +2172,7 @@ LATEX_EMOJI_DIRECTORY = # Configuration options related to the RTF output #--------------------------------------------------------------------------- -# If the GENERATE_RTF tag is set to YES, doxygen will generate RTF output. The +# If the GENERATE_RTF tag is set to YES, Doxygen will generate RTF output. The # RTF output is optimized for Word 97 and may not look too pretty with other RTF # readers/editors. # The default value is: NO. @@ -2113,7 +2187,7 @@ GENERATE_RTF = NO RTF_OUTPUT = rtf -# If the COMPACT_RTF tag is set to YES, doxygen generates more compact RTF +# If the COMPACT_RTF tag is set to YES, Doxygen generates more compact RTF # documents. This may be useful for small projects and may help to save some # trees in general. # The default value is: NO. @@ -2133,18 +2207,18 @@ COMPACT_RTF = NO RTF_HYPERLINKS = NO -# Load stylesheet definitions from file. Syntax is similar to doxygen's +# Load stylesheet definitions from file. Syntax is similar to Doxygen's # configuration file, i.e. a series of assignments. You only have to provide # replacements, missing definitions are set to their default value. # # See also section "Doxygen usage" for information on how to generate the -# default style sheet that doxygen normally uses. +# default style sheet that Doxygen normally uses. # This tag requires that the tag GENERATE_RTF is set to YES. RTF_STYLESHEET_FILE = # Set optional variables used in the generation of an RTF document. Syntax is -# similar to doxygen's configuration file. A template extensions file can be +# similar to Doxygen's configuration file. A template extensions file can be # generated using doxygen -e rtf extensionFile. # This tag requires that the tag GENERATE_RTF is set to YES. @@ -2162,7 +2236,7 @@ RTF_EXTRA_FILES = # Configuration options related to the man page output #--------------------------------------------------------------------------- -# If the GENERATE_MAN tag is set to YES, doxygen will generate man pages for +# If the GENERATE_MAN tag is set to YES, Doxygen will generate man pages for # classes and files. # The default value is: NO. @@ -2193,7 +2267,7 @@ MAN_EXTENSION = .3 MAN_SUBDIR = -# If the MAN_LINKS tag is set to YES and doxygen generates man output, then it +# If the MAN_LINKS tag is set to YES and Doxygen generates man output, then it # will generate one additional man file for each entity documented in the real # man page(s). These additional files only source the real man page, but without # them the man command would be unable to find the correct page. @@ -2206,7 +2280,7 @@ MAN_LINKS = NO # Configuration options related to the XML output #--------------------------------------------------------------------------- -# If the GENERATE_XML tag is set to YES, doxygen will generate an XML file that +# If the GENERATE_XML tag is set to YES, Doxygen will generate an XML file that # captures the structure of the code including all documentation. # The default value is: NO. @@ -2220,7 +2294,7 @@ GENERATE_XML = NO XML_OUTPUT = xml -# If the XML_PROGRAMLISTING tag is set to YES, doxygen will dump the program +# If the XML_PROGRAMLISTING tag is set to YES, Doxygen will dump the program # listings (including syntax highlighting and cross-referencing information) to # the XML output. Note that enabling this will significantly increase the size # of the XML output. @@ -2229,7 +2303,7 @@ XML_OUTPUT = xml XML_PROGRAMLISTING = YES -# If the XML_NS_MEMB_FILE_SCOPE tag is set to YES, doxygen will include +# If the XML_NS_MEMB_FILE_SCOPE tag is set to YES, Doxygen will include # namespace members in file scope as well, matching the HTML output. # The default value is: NO. # This tag requires that the tag GENERATE_XML is set to YES. @@ -2240,7 +2314,7 @@ XML_NS_MEMB_FILE_SCOPE = NO # Configuration options related to the DOCBOOK output #--------------------------------------------------------------------------- -# If the GENERATE_DOCBOOK tag is set to YES, doxygen will generate Docbook files +# If the GENERATE_DOCBOOK tag is set to YES, Doxygen will generate Docbook files # that can be used to generate PDF. # The default value is: NO. @@ -2258,7 +2332,7 @@ DOCBOOK_OUTPUT = docbook # Configuration options for the AutoGen Definitions output #--------------------------------------------------------------------------- -# If the GENERATE_AUTOGEN_DEF tag is set to YES, doxygen will generate an +# If the GENERATE_AUTOGEN_DEF tag is set to YES, Doxygen will generate an # AutoGen Definitions (see https://autogen.sourceforge.net/) file that captures # the structure of the code including all documentation. Note that this feature # is still experimental and incomplete at the moment. @@ -2270,8 +2344,8 @@ GENERATE_AUTOGEN_DEF = NO # Configuration options related to Sqlite3 output #--------------------------------------------------------------------------- -# If the GENERATE_SQLITE3 tag is set to YES doxygen will generate a Sqlite3 -# database with symbols found by doxygen stored in tables. +# If the GENERATE_SQLITE3 tag is set to YES Doxygen will generate a Sqlite3 +# database with symbols found by Doxygen stored in tables. # The default value is: NO. GENERATE_SQLITE3 = NO @@ -2285,7 +2359,7 @@ GENERATE_SQLITE3 = NO SQLITE3_OUTPUT = sqlite3 # The SQLITE3_RECREATE_DB tag is set to YES, the existing doxygen_sqlite3.db -# database file will be recreated with each doxygen run. If set to NO, doxygen +# database file will be recreated with each Doxygen run. If set to NO, Doxygen # will warn if a database file is already found and not modify it. # The default value is: YES. # This tag requires that the tag GENERATE_SQLITE3 is set to YES. @@ -2296,7 +2370,7 @@ SQLITE3_RECREATE_DB = YES # Configuration options related to the Perl module output #--------------------------------------------------------------------------- -# If the GENERATE_PERLMOD tag is set to YES, doxygen will generate a Perl module +# If the GENERATE_PERLMOD tag is set to YES, Doxygen will generate a Perl module # file that captures the structure of the code including all documentation. # # Note that this feature is still experimental and incomplete at the moment. @@ -2304,7 +2378,7 @@ SQLITE3_RECREATE_DB = YES GENERATE_PERLMOD = NO -# If the PERLMOD_LATEX tag is set to YES, doxygen will generate the necessary +# If the PERLMOD_LATEX tag is set to YES, Doxygen will generate the necessary # Makefile rules, Perl scripts and LaTeX code to be able to generate PDF and DVI # output from the Perl module output. # The default value is: NO. @@ -2334,13 +2408,13 @@ PERLMOD_MAKEVAR_PREFIX = # Configuration options related to the preprocessor #--------------------------------------------------------------------------- -# If the ENABLE_PREPROCESSING tag is set to YES, doxygen will evaluate all +# If the ENABLE_PREPROCESSING tag is set to YES, Doxygen will evaluate all # C-preprocessor directives found in the sources and include files. # The default value is: YES. ENABLE_PREPROCESSING = YES -# If the MACRO_EXPANSION tag is set to YES, doxygen will expand all macro names +# If the MACRO_EXPANSION tag is set to YES, Doxygen will expand all macro names # in the source code. If set to NO, only conditional compilation will be # performed. Macro expansion can be done in a controlled way by setting # EXPAND_ONLY_PREDEF to YES. @@ -2401,7 +2475,7 @@ PREDEFINED = _DOXYGEN_ \ EXPAND_AS_DEFINED = -# If the SKIP_FUNCTION_MACROS tag is set to YES then doxygen's preprocessor will +# If the SKIP_FUNCTION_MACROS tag is set to YES then Doxygen's preprocessor will # remove all references to function-like macros that are alone on a line, have # an all uppercase name, and do not end with a semicolon. Such function macros # are typically used for boiler-plate code, and will confuse the parser if not @@ -2425,12 +2499,12 @@ SKIP_FUNCTION_MACROS = NO # section "Linking to external documentation" for more information about the use # of tag files. # Note: Each tag file must have a unique name (where the name does NOT include -# the path). If a tag file is not located in the directory in which doxygen is +# the path). If a tag file is not located in the directory in which Doxygen is # run, you must also specify the path to the tagfile here. TAGFILES = -# When a file name is specified after GENERATE_TAGFILE, doxygen will create a +# When a file name is specified after GENERATE_TAGFILE, Doxygen will create a # tag file that is based on the input files it reads. See section "Linking to # external documentation" for more information about the usage of tag files. @@ -2467,7 +2541,7 @@ EXTERNAL_PAGES = YES HIDE_UNDOC_RELATIONS = YES -# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is +# If you set the HAVE_DOT tag to YES then Doxygen will assume the dot tool is # available from the path. This tool is part of Graphviz (see: # https://www.graphviz.org/), a graph visualization toolkit from AT&T and Lucent # Bell Labs. The other options in this section have no effect if this option is @@ -2476,8 +2550,8 @@ HIDE_UNDOC_RELATIONS = YES HAVE_DOT = YES -# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is allowed -# to run in parallel. When set to 0 doxygen will base this on the number of +# The DOT_NUM_THREADS specifies the number of dot invocations Doxygen is allowed +# to run in parallel. When set to 0 Doxygen will base this on the number of # processors available in the system. You can set it explicitly to a value # larger than 0 to get control over the balance between CPU load and processing # speed. @@ -2488,7 +2562,7 @@ DOT_NUM_THREADS = 0 # DOT_COMMON_ATTR is common attributes for nodes, edges and labels of # subgraphs. When you want a differently looking font in the dot files that -# doxygen generates you can specify fontname, fontcolor and fontsize attributes. +# Doxygen generates you can specify fontname, fontcolor and fontsize attributes. # For details please see Node, # Edge and Graph Attributes specification You need to make sure dot is able # to find the font, which can be done by putting it in a standard location or by @@ -2522,7 +2596,7 @@ DOT_NODE_ATTR = "shape=box,height=0.2,width=0.4" DOT_FONTPATH = -# If the CLASS_GRAPH tag is set to YES or GRAPH or BUILTIN then doxygen will +# If the CLASS_GRAPH tag is set to YES or GRAPH or BUILTIN then Doxygen will # generate a graph for each documented class showing the direct and indirect # inheritance relations. In case the CLASS_GRAPH tag is set to YES or GRAPH and # HAVE_DOT is enabled as well, then dot will be used to draw the graph. In case @@ -2539,7 +2613,7 @@ DOT_FONTPATH = CLASS_GRAPH = YES -# If the COLLABORATION_GRAPH tag is set to YES then doxygen will generate a +# If the COLLABORATION_GRAPH tag is set to YES then Doxygen will generate a # graph for each documented class showing the direct and indirect implementation # dependencies (inheritance, containment, and class references variables) of the # class with other documented classes. Explicit enabling a collaboration graph, @@ -2551,7 +2625,7 @@ CLASS_GRAPH = YES COLLABORATION_GRAPH = YES -# If the GROUP_GRAPHS tag is set to YES then doxygen will generate a graph for +# If the GROUP_GRAPHS tag is set to YES then Doxygen will generate a graph for # groups, showing the direct groups dependencies. Explicit enabling a group # dependency graph, when GROUP_GRAPHS is set to NO, can be accomplished by means # of the command \groupgraph. Disabling a directory graph can be accomplished by @@ -2562,7 +2636,7 @@ COLLABORATION_GRAPH = YES GROUP_GRAPHS = YES -# If the UML_LOOK tag is set to YES, doxygen will generate inheritance and +# If the UML_LOOK tag is set to YES, Doxygen will generate inheritance and # collaboration diagrams in a style similar to the OMG's Unified Modeling # Language. # The default value is: NO. @@ -2583,10 +2657,19 @@ UML_LOOK = NO UML_LIMIT_NUM_FIELDS = 10 -# If the DOT_UML_DETAILS tag is set to NO, doxygen will show attributes and +# If the UML_LOOK tag is enabled, field labels are shown along the edge between +# two class nodes. If there are many fields and many nodes the graph may become +# too cluttered. The UML_MAX_EDGE_LABELS threshold limits the number of items to +# make the size more manageable. Set this to 0 for no limit. +# Minimum value: 0, maximum value: 100, default value: 10. +# This tag requires that the tag UML_LOOK is set to YES. + +UML_MAX_EDGE_LABELS = 10 + +# If the DOT_UML_DETAILS tag is set to NO, Doxygen will show attributes and # methods without types and arguments in the UML graphs. If the DOT_UML_DETAILS -# tag is set to YES, doxygen will add type and arguments for attributes and -# methods in the UML graphs. If the DOT_UML_DETAILS tag is set to NONE, doxygen +# tag is set to YES, Doxygen will add type and arguments for attributes and +# methods in the UML graphs. If the DOT_UML_DETAILS tag is set to NONE, Doxygen # will not generate fields with class member information in the UML graphs. The # class diagrams will look similar to the default class diagrams but using UML # notation for the relationships. @@ -2614,7 +2697,7 @@ DOT_WRAP_THRESHOLD = 17 TEMPLATE_RELATIONS = NO # If the INCLUDE_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are set to -# YES then doxygen will generate a graph for each documented file showing the +# YES then Doxygen will generate a graph for each documented file showing the # direct and indirect include dependencies of the file with other documented # files. Explicit enabling an include graph, when INCLUDE_GRAPH is is set to NO, # can be accomplished by means of the command \includegraph. Disabling an @@ -2625,7 +2708,7 @@ TEMPLATE_RELATIONS = NO INCLUDE_GRAPH = YES # If the INCLUDED_BY_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are -# set to YES then doxygen will generate a graph for each documented file showing +# set to YES then Doxygen will generate a graph for each documented file showing # the direct and indirect include dependencies of the file with other documented # files. Explicit enabling an included by graph, when INCLUDED_BY_GRAPH is set # to NO, can be accomplished by means of the command \includedbygraph. Disabling @@ -2636,7 +2719,7 @@ INCLUDE_GRAPH = YES INCLUDED_BY_GRAPH = YES -# If the CALL_GRAPH tag is set to YES then doxygen will generate a call +# If the CALL_GRAPH tag is set to YES then Doxygen will generate a call # dependency graph for every global function or class method. # # Note that enabling this option will significantly increase the time of a run. @@ -2648,7 +2731,7 @@ INCLUDED_BY_GRAPH = YES CALL_GRAPH = NO -# If the CALLER_GRAPH tag is set to YES then doxygen will generate a caller +# If the CALLER_GRAPH tag is set to YES then Doxygen will generate a caller # dependency graph for every global function or class method. # # Note that enabling this option will significantly increase the time of a run. @@ -2660,14 +2743,14 @@ CALL_GRAPH = NO CALLER_GRAPH = NO -# If the GRAPHICAL_HIERARCHY tag is set to YES then doxygen will graphical +# If the GRAPHICAL_HIERARCHY tag is set to YES then Doxygen will graphical # hierarchy of all classes instead of a textual one. # The default value is: YES. # This tag requires that the tag HAVE_DOT is set to YES. GRAPHICAL_HIERARCHY = YES -# If the DIRECTORY_GRAPH tag is set to YES then doxygen will show the +# If the DIRECTORY_GRAPH tag is set to YES then Doxygen will show the # dependencies a directory has on other directories in a graphical way. The # dependency relations are determined by the #include relations between the # files in the directories. Explicit enabling a directory graph, when @@ -2690,24 +2773,29 @@ DIR_GRAPH_MAX_DEPTH = 1 # generated by dot. For an explanation of the image formats see the section # output formats in the documentation of the dot tool (Graphviz (see: # https://www.graphviz.org/)). -# Note: If you choose svg you need to set HTML_FILE_EXTENSION to xhtml in order -# to make the SVG files visible in IE 9+ (other browsers do not have this -# requirement). +# +# Note the formats svg:cairo and svg:cairo:cairo cannot be used in combination +# with INTERACTIVE_SVG (the INTERACTIVE_SVG will be set to NO). # Possible values are: png, jpg, gif, svg, png:gd, png:gd:gd, png:cairo, -# png:cairo:gd, png:cairo:cairo, png:cairo:gdiplus, png:gdiplus and -# png:gdiplus:gdiplus. +# png:cairo:gd, png:cairo:cairo, png:cairo:gdiplus, png:gdiplus, +# png:gdiplus:gdiplus, svg:cairo, svg:cairo:cairo, svg:svg, svg:svg:core, +# gif:cairo, gif:cairo:gd, gif:cairo:gdiplus, gif:gdiplus, gif:gdiplus:gdiplus, +# gif:gd, gif:gd:gd, jpg:cairo, jpg:cairo:gd, jpg:cairo:gdiplus, jpg:gd, +# jpg:gd:gd, jpg:gdiplus and jpg:gdiplus:gdiplus. # The default value is: png. # This tag requires that the tag HAVE_DOT is set to YES. DOT_IMAGE_FORMAT = svg -# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to -# enable generation of interactive SVG images that allow zooming and panning. +# If DOT_IMAGE_FORMAT is set to svg or svg:svg or svg:svg:core, then this option +# can be set to YES to enable generation of interactive SVG images that allow +# zooming and panning. # # Note that this requires a modern browser other than Internet Explorer. Tested # and working are Firefox, Chrome, Safari, and Opera. -# Note: For IE 9+ you need to set HTML_FILE_EXTENSION to xhtml in order to make -# the SVG files visible. Older versions of IE do not have SVG support. +# +# Note This option will be automatically disabled when DOT_IMAGE_FORMAT is set +# to svg:cairo or svg:cairo:cairo. # The default value is: NO. # This tag requires that the tag HAVE_DOT is set to YES. @@ -2726,7 +2814,7 @@ DOT_PATH = DOTFILE_DIRS = -# You can include diagrams made with dia in doxygen documentation. Doxygen will +# You can include diagrams made with dia in Doxygen documentation. Doxygen will # then run dia to produce the diagram and insert it in the documentation. The # DIA_PATH tag allows you to specify the directory where the dia binary resides. # If left empty dia is assumed to be found in the default search path. @@ -2739,7 +2827,7 @@ DIA_PATH = DIAFILE_DIRS = -# When using plantuml, the PLANTUML_JAR_PATH tag should be used to specify the +# When using PlantUML, the PLANTUML_JAR_PATH tag should be used to specify the # path where java can find the plantuml.jar file or to the filename of jar file # to be used. If left blank, it is assumed PlantUML is not used or called during # a preprocessing step. Doxygen will generate a warning when it encounters a @@ -2747,19 +2835,25 @@ DIAFILE_DIRS = PLANTUML_JAR_PATH = -# When using plantuml, the PLANTUML_CFG_FILE tag can be used to specify a -# configuration file for plantuml. +# When using PlantUML, the PLANTUML_CFG_FILE tag can be used to specify a +# configuration file for PlantUML. PLANTUML_CFG_FILE = -# When using plantuml, the specified paths are searched for files specified by -# the !include statement in a plantuml block. +# When using PlantUML, the specified paths are searched for files specified by +# the !include statement in a PlantUML block. PLANTUML_INCLUDE_PATH = +# The PLANTUMLFILE_DIRS tag can be used to specify one or more directories that +# contain PlantUml files that are included in the documentation (see the +# \plantumlfile command). + +PLANTUMLFILE_DIRS = + # The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of nodes # that will be shown in the graph. If the number of nodes in a graph becomes -# larger than this value, doxygen will truncate the graph, which is visualized +# larger than this value, Doxygen will truncate the graph, which is visualized # by representing a node as a red box. Note that if the number of direct # children of the root node in a graph is already larger than # DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note that @@ -2790,17 +2884,17 @@ MAX_DOT_GRAPH_DEPTH = 0 DOT_MULTI_TARGETS = NO -# If the GENERATE_LEGEND tag is set to YES doxygen will generate a legend page +# If the GENERATE_LEGEND tag is set to YES Doxygen will generate a legend page # explaining the meaning of the various boxes and arrows in the dot generated # graphs. -# Note: This tag requires that UML_LOOK isn't set, i.e. the doxygen internal +# Note: This tag requires that UML_LOOK isn't set, i.e. the Doxygen internal # graphical representation for inheritance and collaboration diagrams is used. # The default value is: YES. # This tag requires that the tag HAVE_DOT is set to YES. GENERATE_LEGEND = YES -# If the DOT_CLEANUP tag is set to YES, doxygen will remove the intermediate +# If the DOT_CLEANUP tag is set to YES, Doxygen will remove the intermediate # files that are used to generate the various graphs. # # Note: This setting is not only used for dot files but also for msc temporary @@ -2809,11 +2903,11 @@ GENERATE_LEGEND = YES DOT_CLEANUP = YES -# You can define message sequence charts within doxygen comments using the \msc -# command. If the MSCGEN_TOOL tag is left empty (the default), then doxygen will +# You can define message sequence charts within Doxygen comments using the \msc +# command. If the MSCGEN_TOOL tag is left empty (the default), then Doxygen will # use a built-in version of mscgen tool to produce the charts. Alternatively, # the MSCGEN_TOOL tag can also specify the name an external tool. For instance, -# specifying prog as the value, doxygen will call the tool as prog -T +# specifying prog as the value, Doxygen will call the tool as prog -T # -o . The external tool should support # output file formats "png", "eps", "svg", and "ismap". diff --git a/fpsdk_doc/fpsdk-overview.drawio.svg b/fpsdk_doc/fpsdk-overview.drawio.svg index 0906bd4b..5b98e1b1 100644 --- a/fpsdk_doc/fpsdk-overview.drawio.svg +++ b/fpsdk_doc/fpsdk-overview.drawio.svg @@ -1,4 +1,4 @@ - + @@ -287,7 +287,7 @@ - +
@@ -611,7 +611,7 @@ - +
@@ -1059,13 +1059,13 @@ - + -
+
utils @@ -1073,15 +1073,15 @@
- + utils - - + + @@ -1137,7 +1137,7 @@ -
+
Noetic @@ -1461,6 +1461,58 @@ + + + + + + + + +
+
+
+ ros1 +
+
+
+ + + ros1 + + + + + + + + + + + + + + + + +
+
+
+ bagwriter +
+
+
+ + + bagwriter + + + + + + + + diff --git a/fpsdk_ros1/CMakeLists.txt b/fpsdk_ros1/CMakeLists.txt index 371a4bcc..dd98d5c5 100644 --- a/fpsdk_ros1/CMakeLists.txt +++ b/fpsdk_ros1/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_ros1 LANGUAGES CXX @@ -34,35 +36,16 @@ if(NOT TARGET fpsdk_common) find_package(fpsdk_common REQUIRED) endif() -set(FP_ROS1_INCLUDE_DIRS "") -set(FP_ROS1_LIBRARIES "") - -find_package(roscpp REQUIRED) -list(APPEND FP_ROS1_INCLUDE_DIRS ${roscpp_INCLUDE_DIRS}) -list(APPEND FP_ROS1_LIBRARIES ${roscpp_LIBRARIES}) - -find_package(rosbag REQUIRED) -list(APPEND FP_ROS1_INCLUDE_DIRS ${rosbag_INCLUDE_DIRS}) -list(APPEND FP_ROS1_LIBRARIES ${rosbag_LIBRARIES}) - -find_package(std_msgs REQUIRED) -list(APPEND FP_ROS1_INCLUDE_DIRS ${std_msgs_INCLUDE_DIRS}) -list(APPEND FP_ROS1_LIBRARIES ${std_msgs_LIBRARIES}) - -find_package(topic_tools REQUIRED) -list(APPEND FP_ROS1_INCLUDE_DIRS ${topic_tools_INCLUDE_DIRS}) -list(APPEND FP_ROS1_LIBRARIES ${topic_tools_LIBRARIES}) - -find_package(genmsg REQUIRED) -list(APPEND FP_ROS1_INCLUDE_DIRS ${genmsg_INCLUDE_DIRS}) -list(APPEND FP_ROS1_LIBRARIES ${genmsg_LIBRARIES}) - - -list(REMOVE_DUPLICATES FP_ROS1_INCLUDE_DIRS) -list(REMOVE_DUPLICATES FP_ROS1_LIBRARIES) - -message(STATUS "fpsdk: FP_ROS1_INCLUDE_DIRS=${FP_ROS1_INCLUDE_DIRS}") -message(STATUS "fpsdk: FP_ROS1_LIBRARIES=${FP_ROS1_LIBRARIES}") +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) +fpsdk_cmake_find_ros1_package(genmsg) # MESSAGES ============================================================================================================= @@ -71,7 +54,7 @@ file(GLOB MSG_FILES RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} msg/*.msg) # /opt/ros/noetic/share/genmsg/cmake/genmsg-extras.cmake add_message_files(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} FILES ${MSG_FILES}) -generate_messages(LANGS gencpp genpy DEPENDENCIES std_msgs) +generate_messages(LANGS gencpp genpy) # DEPENDENCIES std_msgs sensor_msgs) # SHARED LIBRARY ======================================================================================================= @@ -86,14 +69,21 @@ target_include_directories(${PROJECT_NAME} $ $ $ # generate_message() puts them here - $ # generate_message() puts them here - ${FP_ROS1_INCLUDE_DIRS} ) target_link_libraries(${PROJECT_NAME} PUBLIC fpsdk_common - ${FP_ROS1_LIBRARIES} + 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} @@ -172,9 +162,9 @@ install( include(../fpsdk_common/cmake/testing.cmake) -add_gtest(TARGET bagwriter_test SOURCES test/bagwriter_test.cpp LINK_LIBS ${PROJECT_NAME}) -add_gtest(TARGET msgs_test SOURCES test/msgs_test.cpp LINK_LIBS ${PROJECT_NAME}) -add_gtest(TARGET utils_test SOURCES test/utils_test.cpp LINK_LIBS ${PROJECT_NAME}) +add_gtest(TARGET bagwriter_test SOURCES test/bagwriter_test.cpp LINK_LIBS ${PROJECT_NAME} ros1::rosconsole ros1::rosbag) +add_gtest(TARGET msgs_test SOURCES test/msgs_test.cpp LINK_LIBS ${PROJECT_NAME} ros1::rosconsole) +add_gtest(TARGET utils_test SOURCES test/utils_test.cpp LINK_LIBS ${PROJECT_NAME} ros1::rosconsole) # ====================================================================================================================== diff --git a/fpsdk_ros1/cmake/config.cmake.in b/fpsdk_ros1/cmake/config.cmake.in index 73510c6a..9463d461 100644 --- a/fpsdk_ros1/cmake/config.cmake.in +++ b/fpsdk_ros1/cmake/config.cmake.in @@ -12,18 +12,8 @@ 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@;@FP_ROS1_INCLUDE_DIRS@") +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@;@FP_ROS1_LIBRARIES@") - -# ROS libraries dir, for setting RPATH in binaries if desired -SET(@PROJECT_NAME@_ROS_LIBRARIES_DIRS "") -FOREACH(cand ${@PROJECT_NAME@_LIBRARIES}) - get_filename_component(cand ${cand} DIRECTORY) - if("${cand}" MATCHES "/ros/") - list(APPEND @PROJECT_NAME@_ROS_LIBRARIES_DIRS "${cand}") - endif() -ENDFOREACH() -list(REMOVE_DUPLICATES @PROJECT_NAME@_ROS_LIBRARIES_DIRS) +set(@PROJECT_NAME@_LIBRARIES "@TARGET1@") diff --git a/fpsdk_ros1/include/fpsdk_ros1/bagwriter.hpp b/fpsdk_ros1/include/fpsdk_ros1/bagwriter.hpp index e418095b..068992cd 100644 --- a/fpsdk_ros1/include/fpsdk_ros1/bagwriter.hpp +++ b/fpsdk_ros1/include/fpsdk_ros1/bagwriter.hpp @@ -29,6 +29,7 @@ #include "fpsdk_ros1/ext/rosbag_bag.hpp" /* Fixposition SDK */ +#include #include /* PACKAGE */ @@ -53,12 +54,12 @@ class BagWriter /** * @brief Open bag for writing * - * @param path Path/filename of the bag file - * @param compress Compress bag, 0 = no compression, 1 = LZ4, 2+ = BZ2 + * @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); + bool Open(const std::string& path, const int compress = 0); /** * @brief Close bag @@ -66,95 +67,68 @@ class BagWriter void Close(); /** - * @brief Write a message to the bag, do nothing if no bag is open - * - * @param[in] msg The message - * @param[in] topic Optional topic name, if not given the original topic name is used - * - * The bag record time of the msg is used in the output bag. This time is stored and used for writing messages - * that have no time (templated method below). - */ - void WriteMessage(const rosbag::MessageInstance& msg, const std::string& topic = ""); - - /** - * @brief Write a message to the bag, do nothing if no bag is open + * @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 Optional bag record time, if ros::Time() is given the time from the last written message - * (see method above) is used + * @param[in] time Bag record time + * + * @returns true if message was added, false otherwise (message definition missing) */ template - void WriteMessage(const T& msg, const std::string& topic, const ros::Time& time = {}) + bool WriteMessage(const T& msg, const std::string& topic, const ros::Time& time) { + bool ok = false; if (bag_) { - if (!time.isZero()) { - time_ = time; + try { + bag_->write(topic, time, msg); + ok = true; + } catch (const rosbag::BagException& ex) { + WARNING("BagWriter: write fail: %s", ex.what()); } - bag_->write(topic, time_, msg); } + return ok; } - using RosTime = fpsdk::common::time::RosTime; //!< Shortcut - /** - * @brief Write a message to the bag, do nothing if no bag is open + * @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 Optional bag record time, if ros::Time() is given the time from the last written message - * (see method above) is used + * @param[in] time Bag record time + * + * @returns true if message was added, false otherwise (message definition missing) */ template - void WriteMessage(const T& msg, const std::string& topic, const RosTime& time = {}) + bool WriteMessage(const T& msg, const std::string& topic, const common::time::RosTime& time = {}) { - WriteMessage(msg, topic, ros::Time(time.sec_, time.nsec_)); + return WriteMessage(msg, topic, ros::Time(time.sec_, time.nsec_)); } /** - * @brief Add ROS message definition + * @brief Add ROS message definition from .fpl * * @note No checks on the provided data are done! * - * @param[in] topic The topic name - * @param[in] msg_name The message name (a.k.a. data type) - * @param[in] msg_md5 The message MD5 sum - * @param[in] msg_def The message definition - */ - void AddMsgDef( - const std::string& topic, const std::string& msg_name, const std::string& msg_md5, const std::string& msg_def); - - /** - * @brief Write raw binary (serialised) message - * - * @note No checks on the provided data are done! - * - * @param[in] data The binary message data - * @param[in] topic The topic name - * @param[in] time Optional bag record time, if not given the time from the last written message is used - * - * @returns true if message was added, false otherwise (message definition missing) + * @param[in] rosmsgdef The message definition */ - bool WriteMessage(const std::vector& data, const std::string& topic, const ros::Time& time = {}); + void AddMsgDef(const common::fpl::RosMsgDef& rosmsgdef); /** - * @brief Write raw binary (serialised) message + * @brief Write message from .fpl * * @note No checks on the provided data are done! * - * @param[in] data The binary message data - * @param[in] topic The topic name - * @param[in] time Optional bag record time, if not given the time from the last written message is used + * @param[in] rosmsgbin The recorded message * * @returns true if message was added, false otherwise (message definition missing) */ - bool WriteMessage(const std::vector& data, const std::string& topic, const RosTime& time = {}); + bool WriteMessage(const common::fpl::RosMsgBin& rosmsgbin); private: std::unique_ptr bag_; //!< Bag file handle - ros::Time time_; //!< Last known bag record time std::map> msg_defs_; //!< Message definitions (connection headers) }; diff --git a/fpsdk_ros1/src/bagwriter.cpp b/fpsdk_ros1/src/bagwriter.cpp index 54730430..0c461d6a 100644 --- a/fpsdk_ros1/src/bagwriter.cpp +++ b/fpsdk_ros1/src/bagwriter.cpp @@ -8,12 +8,13 @@ * \endverbatim * * @file - * @brief Fixposition SDK: ROS1 bag writer + * @brief Fixposition SDK: ROS2 bag writer */ /* LIBC/STL */ /* EXTERNAL */ +#include "fpsdk_ros1/ext/ros.hpp" #include "fpsdk_ros1/ext/topic_tools.hpp" /* Fixposition SDK */ @@ -52,8 +53,8 @@ bool BagWriter::Open(const std::string& path, const int compress) } try { bag_->open(path, rosbag::bagmode::Write); - } catch (const rosbag::BagException& e) { - WARNING("BagWriter: fail open %s: %s", path.c_str(), e.what()); + } catch (const rosbag::BagException& ex) { + WARNING("BagWriter: open fail %s: %s", path.c_str(), ex.what()); bag_.reset(); return false; } @@ -73,46 +74,32 @@ void BagWriter::Close() // --------------------------------------------------------------------------------------------------------------------- -void BagWriter::WriteMessage(const rosbag::MessageInstance& msg, const std::string& topic) +void BagWriter::AddMsgDef(const common::fpl::RosMsgDef& rosmsgdef) { - time_ = msg.getTime(); - if (bag_) { - bag_->write(topic.empty() ? msg.getTopic() : topic, time_, msg); - } -} - -// --------------------------------------------------------------------------------------------------------------------- - -void BagWriter::AddMsgDef( - const std::string& topic, const std::string& msg_name, const std::string& msg_md5, const std::string& msg_def) -{ - if (msg_defs_.find(topic) == msg_defs_.end()) { - DEBUG("BagWriter: %s, %s, %s, <%" PRIu64 ">", topic.c_str(), msg_name.c_str(), msg_md5.c_str(), msg_def.size()); + 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"), msg_def); - hdr->emplace(std::string("topic"), topic); - hdr->emplace(std::string("md5sum"), msg_md5); - hdr->emplace(std::string("type"), msg_name); - msg_defs_.emplace(topic, hdr); + 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 std::vector& data, const std::string& topic, const RosTime& time) +bool BagWriter::WriteMessage(const common::fpl::RosMsgBin& rosmsgbin) { - return WriteMessage(data, topic, ros::Time(time.sec_, time.nsec_)); -} - -bool BagWriter::WriteMessage(const std::vector& data, const std::string& topic, const ros::Time& time) -{ - if (!time.isZero()) { - time_ = time; + 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(topic); + auto msg_defs_entry = msg_defs_.find(rosmsgbin.topic_name_); if (msg_defs_entry == msg_defs_.end()) { - ROS_WARN("BagWriter: missing message definition for %s", topic.c_str()); + WARNING("BagWriter: missing message definition for %s", rosmsgbin.topic_name_.c_str()); return false; } @@ -126,13 +113,16 @@ bool BagWriter::WriteMessage(const std::vector& data, const std::string }; // clang-format on topic_tools::ShapeShifter ros_msg; - ShapeShifterReadHelper stream(data.data(), data.size()); + ShapeShifterReadHelper stream(rosmsgbin.msg_data_.data(), rosmsgbin.msg_data_.size()); ros_msg.read(stream); try { - bag_->write(topic, time_, ros_msg, msg_defs_entry->second); + 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) { - ROS_WARN("BagWriter: write fail: %s", e.what()); + WARNING("BagWriter: write fail: %s", e.what()); return false; } diff --git a/fpsdk_ros2/CMakeLists.txt b/fpsdk_ros2/CMakeLists.txt index f9fc26a7..5eba77fa 100644 --- a/fpsdk_ros2/CMakeLists.txt +++ b/fpsdk_ros2/CMakeLists.txt @@ -1,4 +1,5 @@ # GENERAL ============================================================================================================== +message(STATUS "fpsdk: ----- ${CMAKE_CURRENT_SOURCE_DIR} -----") cmake_minimum_required(VERSION 3.16) include(../fpsdk_common/cmake/setup.cmake) @@ -34,19 +35,12 @@ if(NOT TARGET fpsdk_common) find_package(fpsdk_common REQUIRED) endif() -set(FP_ROS2_INCLUDE_DIRS "") -set(FP_ROS2_LIBRARIES "") - find_package(rclcpp REQUIRED) -list(APPEND FP_ROS2_INCLUDE_DIRS ${rclcpp_INCLUDE_DIRS}) -list(APPEND FP_ROS2_LIBRARIES ${rclcpp_LIBRARIES}) - - -list(REMOVE_DUPLICATES FP_ROS2_INCLUDE_DIRS) -list(REMOVE_DUPLICATES FP_ROS2_LIBRARIES) - -message(STATUS "fpsdk: FP_ROS2_INCLUDE_DIRS=${FP_ROS2_INCLUDE_DIRS}") -message(STATUS "fpsdk: FP_ROS2_LIBRARIES=${FP_ROS2_LIBRARIES}") +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) # SHARED LIBRARY ======================================================================================================= @@ -58,13 +52,20 @@ target_include_directories(${PROJECT_NAME} PUBLIC $ $ - ${FP_ROS2_INCLUDE_DIRS} ) +set(FPSDK_COMMON_TRANS_DEPS "fpsdk_common;${rclcpp_TARGETS};${rosbag2_cpp_TARGETS}") + target_link_libraries(${PROJECT_NAME} PUBLIC fpsdk_common - ${FP_ROS2_LIBRARIES} + ${rclcpp_TARGETS} + ${rosbag2_cpp_TARGETS} + PRIVATE + ${sensor_msgs_TARGETS} + ${geometry_msgs_TARGETS} + ${tf2_msgs_TARGETS} + ${nav_msgs_TARGETS} ) set_target_properties(${PROJECT_NAME} @@ -143,7 +144,9 @@ install( include(../fpsdk_common/cmake/testing.cmake) -add_gtest(TARGET utils_test SOURCES test/utils_test.cpp LINK_LIBS ${PROJECT_NAME}) +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}) # ====================================================================================================================== diff --git a/fpsdk_ros2/cmake/config.cmake.in b/fpsdk_ros2/cmake/config.cmake.in index e1976385..9463d461 100644 --- a/fpsdk_ros2/cmake/config.cmake.in +++ b/fpsdk_ros2/cmake/config.cmake.in @@ -12,18 +12,8 @@ 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@;@FP_ROS2_INCLUDE_DIRS@") +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@;@FP_ROS2_LIBRARIES@") - -# ROS libraries dir, for setting RPATH in binaries if desired -SET(@PROJECT_NAME@_ROS_LIBRARIES_DIRS "") -FOREACH(cand ${@PROJECT_NAME@_LIBRARIES}) - get_filename_component(cand ${cand} DIRECTORY) - if("${cand}" MATCHES "/ros/") - list(APPEND @PROJECT_NAME@_ROS_LIBRARIES_DIRS "${cand}") - endif() -ENDFOREACH() -list(REMOVE_DUPLICATES @PROJECT_NAME@_ROS_LIBRARIES_DIRS) +set(@PROJECT_NAME@_LIBRARIES "@TARGET1@") diff --git a/fpsdk_ros2/doc/doc.hpp b/fpsdk_ros2/doc/doc.hpp index 4515b10c..1522543b 100644 --- a/fpsdk_ros2/doc/doc.hpp +++ b/fpsdk_ros2/doc/doc.hpp @@ -31,7 +31,9 @@ namespace ros2 { @section FPSDK_ROS2_MODULES Modules + - @subpage FPSDK_ROS2_BAGWRITER - @subpage FPSDK_ROS2_UTILS + - @subpage FPSDK_ROS2_ROS1 @section FPSDK_ROS2_LICENSE License diff --git a/fpsdk_ros2/include/fpsdk_ros2/bagwriter.hpp b/fpsdk_ros2/include/fpsdk_ros2/bagwriter.hpp new file mode 100644 index 00000000..4713c01f --- /dev/null +++ b/fpsdk_ros2/include/fpsdk_ros2/bagwriter.hpp @@ -0,0 +1,133 @@ +/** + * \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 new file mode 100644 index 00000000..7a90d66b --- /dev/null +++ b/fpsdk_ros2/include/fpsdk_ros2/ext/msgs.hpp @@ -0,0 +1,16 @@ +// 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 + +#pragma GCC diagnostic pop +#endif // __FPSDK_ROS2_EXT_MSGS_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 new file mode 100644 index 00000000..f7d9cb10 --- /dev/null +++ b/fpsdk_ros2/include/fpsdk_ros2/ext/rosbag2_cpp_writer.hpp @@ -0,0 +1,11 @@ +// 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/ros1.hpp b/fpsdk_ros2/include/fpsdk_ros2/ros1.hpp new file mode 100644 index 00000000..c86aa1c4 --- /dev/null +++ b/fpsdk_ros2/include/fpsdk_ros2/ros1.hpp @@ -0,0 +1,204 @@ +/** + * \verbatim + * ___ ___ + * \ \ / / + * \ \/ / Copyright (c) Fixposition AG + * / /\ \ License: see the LICENSE file + * /__/ \__\ + * \endverbatim + * + * @file + * @brief Fixposition SDK: ROS2 types conversion from ROS1 + * + * @page FPSDK_ROS2_ROS1 ROS2 types conversion from ROS1 + * + * **API**: fpsdk_ros2/ros1.hpp and fpsdk::ros2::ros1 + * + */ +#ifndef __FPSDK_ROS2_ROS1_HPP__ +#define __FPSDK_ROS2_ROS1_HPP__ + +/* LIBC/STL */ +#include + +/* EXTERNAL */ +#include + +/* Fixposition SDK */ +#include + +/* PACKAGE */ + +namespace fpsdk { +namespace ros2 { +/** + * @brief ROS2 types conversion from ROS1 + */ +namespace ros1 { +/* ****************************************************************************************************************** */ +#ifdef _DOXYGEN_ + +// Dummy documentation +/** + * @brief Convert ROS1 message to ROS2 message + * + * Several conversions in this form are implemented. See the source code for details. + * + * @tparam Ros1MsgT ROS1 message type + * @tparam Ros2MsgT ROS2 message type + * @param[in] ros1 ROS1 message + * @param[out] ros2 ROS2 message + */ +template +void Ros1ToRos2(Ros1MsgT& ros1, Ros2MsgT& ros2); + +#else + +inline void Ros1ToRos2(const ros::Time& ros1, builtin_interfaces::msg::Time& ros2) +{ + ros2.sec = ros1.sec; + ros2.nanosec = ros1.nsec; +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void Ros1ToRos2(const boost::array& ros1, std::array& ros2) +{ + for (std::size_t ix = 0; ix < 9; ix++) { + ros2[ix] = ros1[ix]; + } +} + +inline void Ros1ToRos2(const boost::array& ros1, std::array& ros2) +{ + for (std::size_t ix = 0; ix < 36; ix++) { + ros2[ix] = ros1[ix]; + } +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void Ros1ToRos2(const std_msgs::Header& ros1, std_msgs::msg::Header& ros2) +{ + ros2.frame_id = ros1.frame_id; + Ros1ToRos2(ros1.stamp, ros2.stamp); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void Ros1ToRos2(const geometry_msgs::Quaternion& ros1, geometry_msgs::msg::Quaternion& ros2) +{ + ros2.x = ros1.x; + ros2.y = ros1.y; + ros2.z = ros1.z; + ros2.w = ros1.w; +} + +inline void Ros1ToRos2(const geometry_msgs::Vector3& ros1, geometry_msgs::msg::Vector3& ros2) +{ + ros2.x = ros1.x; + ros2.y = ros1.y; + ros2.z = ros1.z; +} + +inline void Ros1ToRos2(const geometry_msgs::Point& ros1, geometry_msgs::msg::Point& ros2) +{ + ros2.x = ros1.x; + ros2.y = ros1.y; + ros2.z = ros1.z; +} + +inline void Ros1ToRos2(const geometry_msgs::Pose& ros1, geometry_msgs::msg::Pose& ros2) +{ + Ros1ToRos2(ros1.position, ros2.position); + Ros1ToRos2(ros1.orientation, ros2.orientation); +} + +inline void Ros1ToRos2(const geometry_msgs::Twist& ros1, geometry_msgs::msg::Twist& ros2) +{ + Ros1ToRos2(ros1.linear, ros2.linear); + Ros1ToRos2(ros1.angular, ros2.angular); +} + +inline void Ros1ToRos2(const geometry_msgs::PoseWithCovariance& ros1, geometry_msgs::msg::PoseWithCovariance& ros2) +{ + Ros1ToRos2(ros1.pose, ros2.pose); + Ros1ToRos2(ros1.covariance, ros2.covariance); +} + +inline void Ros1ToRos2(const geometry_msgs::TwistWithCovariance& ros1, geometry_msgs::msg::TwistWithCovariance& ros2) +{ + Ros1ToRos2(ros1.twist, ros2.twist); + Ros1ToRos2(ros1.covariance, ros2.covariance); +} + +inline void Ros1ToRos2(const geometry_msgs::Transform& ros1, geometry_msgs::msg::Transform& ros2) +{ + Ros1ToRos2(ros1.translation, ros2.translation); + Ros1ToRos2(ros1.rotation, ros2.rotation); +} + +inline void Ros1ToRos2(const geometry_msgs::TransformStamped& ros1, geometry_msgs::msg::TransformStamped& ros2) +{ + Ros1ToRos2(ros1.header, ros2.header); + ros2.child_frame_id = ros1.child_frame_id; + Ros1ToRos2(ros1.transform, ros2.transform); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void Ros1ToRos2(const sensor_msgs::Imu& ros1, sensor_msgs::msg::Imu& ros2) +{ + Ros1ToRos2(ros1.header, ros2.header); + Ros1ToRos2(ros1.orientation, ros2.orientation); + Ros1ToRos2(ros1.orientation_covariance, ros2.orientation_covariance); + Ros1ToRos2(ros1.angular_velocity, ros2.angular_velocity); + Ros1ToRos2(ros1.angular_velocity_covariance, ros2.angular_velocity_covariance); + Ros1ToRos2(ros1.linear_acceleration, ros2.linear_acceleration); + Ros1ToRos2(ros1.linear_acceleration_covariance, ros2.linear_acceleration_covariance); +} + +inline void Ros1ToRos2(const sensor_msgs::Temperature& ros1, sensor_msgs::msg::Temperature& ros2) +{ + Ros1ToRos2(ros1.header, ros2.header); + ros2.temperature = ros1.temperature; + ros2.variance = ros1.variance; +} + +inline void Ros1ToRos2(const sensor_msgs::Image& ros1, sensor_msgs::msg::Image& ros2) +{ + Ros1ToRos2(ros1.header, ros2.header); + ros2.height = ros1.height; + ros2.width = ros1.width; + ros2.encoding = ros1.encoding; + ros2.step = ros1.step; + ros2.data = ros1.data; +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void Ros1ToRos2(const nav_msgs::Odometry& ros1, nav_msgs::msg::Odometry& ros2) +{ + Ros1ToRos2(ros1.header, ros2.header); + ros2.child_frame_id = ros1.child_frame_id; + Ros1ToRos2(ros1.pose, ros2.pose); + Ros1ToRos2(ros1.twist, ros2.twist); +} + +// --------------------------------------------------------------------------------------------------------------------- + +inline void Ros1ToRos2(const tf2_msgs::TFMessage& ros1, tf2_msgs::msg::TFMessage& ros2) +{ + for (auto& tf_ros1 : ros1.transforms) { + geometry_msgs::msg::TransformStamped tf_ros2; + Ros1ToRos2(tf_ros1, tf_ros2); + ros2.transforms.push_back(std::move(tf_ros2)); + } +} + +#endif // !_DOXYGEN_ +/* ****************************************************************************************************************** */ +} // namespace ros1 +} // namespace ros2 +} // namespace fpsdk +#endif // __FPSDK_ROS2_ROS1_HPP__ diff --git a/fpsdk_ros2/package.xml b/fpsdk_ros2/package.xml index 5d72a11f..9a83aabd 100644 --- a/fpsdk_ros2/package.xml +++ b/fpsdk_ros2/package.xml @@ -10,4 +10,9 @@ cmake fpsdk_common + std_msgs + sensor_msgs + geometry_msgs + tf2_msgs + rosbag2_cpp diff --git a/fpsdk_ros2/src/bagwriter.cpp b/fpsdk_ros2/src/bagwriter.cpp new file mode 100644 index 00000000..94c032e3 --- /dev/null +++ b/fpsdk_ros2/src/bagwriter.cpp @@ -0,0 +1,148 @@ +/** + * \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 */ +#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" + +namespace fpsdk { +namespace ros2 { +namespace bagwriter { +/* ****************************************************************************************************************** */ + +BagWriter::BagWriter() +{ +} + +BagWriter::~BagWriter() +{ + Close(); +} + +// --------------------------------------------------------------------------------------------------------------------- + +bool BagWriter::Open(const std::string& path, const int compress) +{ + Close(); + bag_ = std::make_unique(); + rosbag2_storage::StorageOptions opts; + opts.uri = path; + if (compress > 0) { + opts.storage_id = "mcap"; // apt install ros-${ROS_DISTRO}-rosbag2-storage-mcap + opts.storage_preset_profile = (compress > 1 ? "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(), + opts.storage_id.c_str()); + bag_.reset(); + return false; + } + DEBUG("BagWriter: %s", path.c_str()); + + return true; +} + +// --------------------------------------------------------------------------------------------------------------------- + +void BagWriter::Close() +{ + if (bag_) { + bag_->close(); + bag_.reset(); + } +} + +// --------------------------------------------------------------------------------------------------------------------- + +void BagWriter::AddMsgDef(const common::fpl::RosMsgDef& rosmsgdef) +{ + if (rosmsgdef.valid_ && (defs_.find(rosmsgdef.topic_name_) == defs_.end())) { + DEBUG("BagWriter: %s", rosmsgdef.info_.c_str()); + defs_[rosmsgdef.topic_name_] = rosmsgdef; + } +} + +// --------------------------------------------------------------------------------------------------------------------- + +template +inline bool WriteMessageEx( + const common::fpl::RosMsgDef& rosmsgdef, const common::fpl::RosMsgBin& rosmsgbin, ros2::bagwriter::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); + bag.WriteMessage(ros2, rosmsgbin.topic_name_, rosmsgbin.rec_time_); + return true; + } else { + return false; + } +} + +bool BagWriter::WriteMessage(const common::fpl::RosMsgBin& rosmsgbin) +{ + if (!rosmsgbin.valid_) { + return false; + } + + const auto& entry = defs_.find(rosmsgbin.topic_name_); + if (entry == defs_.end()) { + WARNING("BagWriter: missing message definition for %s", rosmsgbin.topic_name_.c_str()); + + WARNING_THR(1000, "Missing ROSMSGDEF for ROSMSGBIN %s", rosmsgbin.info_.c_str()); + return false; + } + + const auto& rosmsgdef = entry->second; + + // For ROS2 we have to instantiate the ROS1 message in order to be able to convert it to the corresponding ROS2 + // message type, which includes the message meta data and definition. This can then be written to the bag. Only some + // conversions are implemented. + try { + if (WriteMessageEx(rosmsgdef, rosmsgbin, *this) || + WriteMessageEx(rosmsgdef, rosmsgbin, *this) || + WriteMessageEx(rosmsgdef, rosmsgbin, *this) || + WriteMessageEx(rosmsgdef, rosmsgbin, *this) || + WriteMessageEx(rosmsgdef, rosmsgbin, *this)) { + } else { + throw std::runtime_error("conversion not implemented"); + } + } catch (std::exception& ex) { + WARNING("BagWriter: write fail: %s %s", rosmsgdef.msg_name_.c_str(), ex.what()); + return false; + } + + return true; +} + +/* ****************************************************************************************************************** */ +} // namespace bagwriter +} // namespace ros2 +} // namespace fpsdk diff --git a/fpsdk_apps/fpltool/fpltool_rosbag.hpp b/fpsdk_ros2/src/ros1.cpp similarity index 57% rename from fpsdk_apps/fpltool/fpltool_rosbag.hpp rename to fpsdk_ros2/src/ros1.cpp index 349eae82..bc36e20f 100644 --- a/fpsdk_apps/fpltool/fpltool_rosbag.hpp +++ b/fpsdk_ros2/src/ros1.cpp @@ -8,34 +8,27 @@ * \endverbatim * * @file - * @brief Fixposition SDK: fpltool rosbag + * @brief Fixposition SDK: ROS2 types conversion from ROS1 */ -#ifndef __FPSDK_APPS_FPLTOOL_FPLTOOL_ROSBAG_HPP__ -#define __FPSDK_APPS_FPLTOOL_FPLTOOL_ROSBAG_HPP__ /* LIBC/STL */ /* EXTERNAL */ /* Fixposition SDK */ +#include /* PACKAGE */ -#include "fpltool_opts.hpp" +#include "fpsdk_ros2/ros1.hpp" namespace fpsdk { -namespace apps { -namespace fpltool { +namespace ros2 { +namespace ros1 { /* ****************************************************************************************************************** */ -/** - * @brief Run FpltoolArgs::Command::ROSBAG - * - * @param[in] opts Options - */ -bool DoRosbag(const FplToolOptions& opts); +// --------------------------------------------------------------------------------------------------------------------- /* ****************************************************************************************************************** */ -} // namespace fpltool -} // namespace apps +} // namespace ros1 +} // namespace ros2 } // namespace fpsdk -#endif // __FPSDK_APPS_FPLTOOL_FPLTOOL_ROSBAG_HPP__ diff --git a/fpsdk_ros2/test/bagwriter_test.cpp b/fpsdk_ros2/test/bagwriter_test.cpp new file mode 100644 index 00000000..fc906112 --- /dev/null +++ b/fpsdk_ros2/test/bagwriter_test.cpp @@ -0,0 +1,46 @@ +/** + * \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 new file mode 100644 index 00000000..0dfc0544 --- /dev/null +++ b/fpsdk_ros2/test/ros1_test.cpp @@ -0,0 +1,46 @@ +/** + * \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(); +}