Extract raw data and provide a means to register in external frame

.circleci/config.yml

@@ -40,31 +40,23 @@ workflows:
       - checkout_build_test:
           resource_class: arm.medium
           arch: arm64v8
-          ros_distro: foxy
-      - checkout_build_test:
-          resource_class: arm.medium
-          arch: arm64v8
-          ros_distro: galactic
+          ros_distro: humble
       - checkout_build_test:
           resource_class: arm.medium
           arch: arm64v8
-          ros_distro: humble
+          ros_distro: iron
       - checkout_build_test:
           resource_class: arm.medium
           arch: arm64v8
           ros_distro: rolling
       - checkout_build_test:
           resource_class: medium
           arch: amd64
-          ros_distro: foxy
-      - checkout_build_test:
-          resource_class: medium
-          arch: amd64
-          ros_distro: galactic
+          ros_distro: humble
       - checkout_build_test:
           resource_class: medium
           arch: amd64
-          ros_distro: humble
+          ros_distro: iron
       - checkout_build_test:
           resource_class: medium
           arch: amd64

.dockerignore

@@ -0,0 +1 @@
+data

.gitignore

@@ -1 +1,53 @@
+# Byte-compiled / optimized / DLL files
+__pycache__/
+*.py[cod]
+
+# C extensions
+*.so
+
+# Distribution / packaging
+bin/
+build/
+develop-eggs/
+dist/
+eggs/
+lib/
+lib64/
+parts/
+sdist/
+var/
+*.egg-info/
+.installed.cfg
+*.egg
+
+# Installer logs
+pip-log.txt
+pip-delete-this-directory.txt
+
+# Unit test / coverage reports
+.tox/
+.coverage
+.cache
+nosetests.xml
+coverage.xml
+
+# Translations
+*.mo
+
+# Mr Developer
+.mr.developer.cfg
+.project
+.pydevproject
+
+# Rope
+.ropeproject
+
+# Django stuff:
+*.log
+*.pot
+
+# Sphinx documentation
+docs/_build/
+
+# Visual studio code files
 .vscode

CMakeLists.txt

@@ -24,6 +24,8 @@ cmake_minimum_required(VERSION 3.5)
 # Set the project name
 project(libsurvive_ros2)
 
+# COMPILATION OPTIONS
+
 # Default to C99
 if(NOT CMAKE_C_STANDARD)
   set(CMAKE_C_STANDARD 99)
@@ -37,101 +39,196 @@ endif()
 # Set compile options
 add_compile_options(-Wall -Wextra -Wpedantic)
 
-# Find the core interface library
+# Cached compilation
+find_program(CCACHE_PROGRAM ccache)
+if(CCACHE_PROGRAM)
+    set_property(GLOBAL PROPERTY RULE_LAUNCH_COMPILE "${CCACHE_PROGRAM}")
+endif()
+
+# DEPENDENCIES
+
+# Obtain a specific version of the libsurvive low-level driver.
 include(ExternalProject)
 externalproject_add(libsurvive
   GIT_REPOSITORY https://github.com/cntools/libsurvive.git
-  GIT_TAG master
+  GIT_TAG d89876a1af0efcfcddeaaf2a282eec8753a1bac4
   CMAKE_ARGS
-    -DCMAKE_INSTALL_PREFIX=${CMAKE_CURRENT_BINARY_DIR}/libsurvive-install
+    -DCMAKE_BINARY_DIR=${CMAKE_CURRENT_BINARY_DIR}
+    -DCMAKE_INSTALL_PREFIX=${CMAKE_INSTALL_PREFIX}
     -DCMAKE_BUILD_TYPE=Release
 )
 
-# Find dependencies
+# Third party libraries
+find_package(Boost REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(GTSAM REQUIRED)
+find_package(TBB REQUIRED)
+find_package(yaml-cpp REQUIRED)
+
+# ROS packages
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_components REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(tf2_ros REQUIRED)
+
+# Interfaces
 find_package(diagnostic_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
-find_package(tf2 REQUIRED)
-find_package(tf2_ros REQUIRED)
 
 # Universally add this components includes
 include_directories(include)
 
-# Add the component
-add_library(libsurvive_ros2_component SHARED
-  src/component.cpp)
-add_dependencies(libsurvive_ros2_component libsurvive)
-target_include_directories(libsurvive_ros2_component PUBLIC
-  ${CMAKE_CURRENT_BINARY_DIR}/libsurvive-install/include
-  ${CMAKE_CURRENT_BINARY_DIR}/libsurvive-install/include/libsurvive
-  ${CMAKE_CURRENT_BINARY_DIR}/libsurvive-install/include/libsurvive/redist)
-target_link_directories(libsurvive_ros2_component PUBLIC
-  ${CMAKE_CURRENT_BINARY_DIR}/libsurvive-install/lib)
-target_link_libraries(libsurvive_ros2_component -lsurvive)
-target_compile_definitions(libsurvive_ros2_component
+# IDL GENERATION
+
+# Generate messages used in this project
+rosidl_generate_interfaces(libsurvive_ros2
+    "msg/Angle.msg"
+    "msg/Lighthouse.msg"
+    "msg/Tracker.msg"
+  DEPENDENCIES
+    diagnostic_msgs
+    geometry_msgs
+    std_msgs)
+rosidl_get_typesupport_target(cpp_typesupport_target
+  libsurvive_ros2 "rosidl_typesupport_cpp")
+
+# BUILDING DRIVER
+
+# Component
+add_library(libsurvive_ros2_driver_component SHARED
+  src/driver_component.cpp
+  src/driver_poser_null.cpp)
+add_dependencies(libsurvive_ros2_driver_component
+  libsurvive)
+target_include_directories(libsurvive_ros2_driver_component PUBLIC
+  ${CMAKE_INSTALL_PREFIX}/include
+  ${CMAKE_INSTALL_PREFIX}/include/libsurvive
+  ${CMAKE_INSTALL_PREFIX}/include/libsurvive/redist)
+target_link_directories(libsurvive_ros2_driver_component PUBLIC
+  ${CMAKE_INSTALL_PREFIX}/lib)
+target_link_libraries(libsurvive_ros2_driver_component
+  "${cpp_typesupport_target}"
+  -lsurvive)
+target_compile_definitions(libsurvive_ros2_driver_component
   PRIVATE "COMPOSITION_BUILDING_DLL")
-ament_target_dependencies(libsurvive_ros2_component
+ament_target_dependencies(libsurvive_ros2_driver_component
   rclcpp
   rclcpp_components
   diagnostic_msgs
   geometry_msgs
   sensor_msgs
   tf2
   tf2_ros)
-rclcpp_components_register_nodes(libsurvive_ros2_component "libsurvive_ros2::Component")
+rclcpp_components_register_nodes(libsurvive_ros2_driver_component
+  "libsurvive_ros2::DriverComponent")
+
+# Node
+add_executable(libsurvive_ros2_driver_node
+  src/driver_node.cpp)
+target_link_libraries(libsurvive_ros2_driver_node
+  libsurvive_ros2_driver_component)
+ament_target_dependencies(libsurvive_ros2_driver_node rclcpp)
+
+# BUILDING POSER
+
+# Component
+add_library(libsurvive_ros2_poser_component SHARED
+  src/poser_component.cpp)
+target_include_directories(libsurvive_ros2_poser_component PUBLIC
+  ${GTSAM_INCLUDE_DIR}
+  ${YAML_CPP_INCLUDE_DIRS}
+  ${Boost_INCLUDE_DIRS}
+  ${TBB_INCLUDE_DIRS})
+target_link_libraries(libsurvive_ros2_poser_component
+  gtsam
+  Eigen3::Eigen
+  ${YAML_CPP_LIBRARIES}
+  "${cpp_typesupport_target}")
+target_compile_definitions(libsurvive_ros2_poser_component
+  PRIVATE "COMPOSITION_BUILDING_DLL")
+target_compile_options(libsurvive_ros2_poser_component
+  PUBLIC -Wno-deprecated-copy -Wno-unused-parameter)
+ament_target_dependencies(libsurvive_ros2_poser_component
+  rclcpp
+  rclcpp_components
+  diagnostic_msgs
+  geometry_msgs
+  sensor_msgs
+  tf2
+  tf2_ros)
+rclcpp_components_register_nodes(libsurvive_ros2_poser_component
+  "libsurvive_ros2::PoserComponent")
 
-# Export the library path because the package installs libraries without exporting them
-if(NOT WIN32)
-  ament_environment_hooks(
-    "${ament_cmake_package_templates_ENVIRONMENT_HOOK_LIBRARY_PATH}")
-endif()
+# Node
+add_executable(libsurvive_ros2_poser_node
+  src/poser_node.cpp)
+target_link_libraries(libsurvive_ros2_poser_node
+  libsurvive_ros2_poser_component)
+ament_target_dependencies(libsurvive_ros2_poser_node rclcpp)
 
-# Manually compose the component into a node
-add_executable(libsurvive_ros2_node
-  src/node.cpp)
-target_link_libraries(libsurvive_ros2_node
-  libsurvive_ros2_component)
-ament_target_dependencies(libsurvive_ros2_node rclcpp)
+# INSTALLATION
 
-# Install libsurvive
-install(
-  DIRECTORY
-    ${CMAKE_CURRENT_BINARY_DIR}/libsurvive-install/
-  DESTINATION
-    ${CMAKE_INSTALL_PREFIX}
-)
+# Install the components
 
-# Install the component
 install(
   TARGETS
-    libsurvive_ros2_component
+    libsurvive_ros2_driver_component
+    libsurvive_ros2_poser_component
   ARCHIVE DESTINATION lib
   LIBRARY DESTINATION lib
   RUNTIME DESTINATION bin)
 
-# Install the node
+# Install the nodes
+
 install(
   TARGETS
-    libsurvive_ros2_node
+    libsurvive_ros2_driver_node
+    libsurvive_ros2_poser_node
   DESTINATION lib/${PROJECT_NAME})
 
-# Install the shared things
+# Install helper programs
+
+install(
+  PROGRAMS
+    scripts/bag_parser
+    scripts/config_merger
+    scripts/offline_optimizer
+  DESTINATION
+    lib/${PROJECT_NAME})
+
+# Install the shared files
+
 install(
   DIRECTORY
-    config
     launch
+    config
   DESTINATION share/${PROJECT_NAME})
 
-# For testing only
+# Install the python package
+
+ament_python_install_package(libsurvive_ros2_py)
+
+# TESTING
+
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
+  find_package(ament_cmake_ros REQUIRED)
+
+  # Lint tests
   ament_lint_auto_find_test_dependencies()
+
+  # Unit tests
+  ament_add_ros_isolated_pytest_test(test_config_tools
+      test/test_config_tools.py
+    TIMEOUT 120)
+
 endif()
 
+# EXPORTS
+
 # Export all include directories and declare the package
 ament_export_include_directories(include)
 ament_package()
-