├── .circleci └── config.yml ├── .devcontainer ├── foxy │ └── devcontainer.json ├── galactic │ └── devcontainer.json ├── humble │ └── devcontainer.json ├── iron │ └── devcontainer.json ├── jazzy │ └── devcontainer.json ├── kilted │ └── devcontainer.json └── rolling │ └── devcontainer.json ├── .github └── FUNDING.yml ├── .gitignore ├── CMakeLists.txt ├── CONTRIBUTING.md ├── Dockerfile ├── LICENSE ├── README.md ├── config └── config.json ├── doc └── foxglove.png ├── docker-compose.yml ├── include └── libsurvive_ros2 │ └── component.hpp ├── launch └── libsurvive_ros2.launch.py ├── package.xml └── src ├── component.cpp └── node.cpp /.circleci/config.yml: -------------------------------------------------------------------------------- 1 | version: 2.1 2 | jobs: 3 | checkout_build_test: 4 | parameters: 5 | resource_class: 6 | default: "medium" 7 | type: string 8 | arch: 9 | default: "amd64" 10 | type: string 11 | ros_distro: 12 | default: "rolling" 13 | type: string 14 | machine: 15 | image: ubuntu-2204:current 16 | resource_class: << parameters.resource_class >> 17 | description: "Checkout, build, and test for << parameters.arch >>/<< parameters.ros_distro >>" 18 | steps: 19 | - checkout 20 | - run: 21 | name: Install docker client 22 | command: | 23 | sudo apt-get install ca-certificates curl gnupg lsb-release -y --no-install-recommends 24 | sudo mkdir -m 0755 -p /etc/apt/keyrings 25 | curl -fsSL https://download.docker.com/linux/ubuntu/gpg | gpg --dearmor | sudo tee /etc/apt/keyrings/docker.gpg > /dev/null 26 | echo "deb [arch=$(dpkg --print-architecture) signed-by=/etc/apt/keyrings/docker.gpg] https://download.docker.com/linux/ubuntu $(lsb_release -cs) stable" | sudo tee /etc/apt/sources.list.d/docker.list > /dev/null 27 | sudo apt-get update 28 | sudo apt-get install docker-ce docker-ce-cli containerd.io docker-buildx-plugin docker-compose-plugin -y --no-install-recommends 29 | - run: 30 | name: Build code 31 | command: | 32 | docker compose build libsurvive_ros2 --build-arg ARCH=<< parameters.arch >> --build-arg ROS_DISTRO=<< parameters.ros_distro >> 33 | - run: 34 | name: Test code 35 | command: | 36 | docker compose run libsurvive_ros2 colcon test --packages-select libsurvive_ros2 --return-code-on-test-failure --event-handlers console_direct+ 37 | workflows: 38 | build: 39 | jobs: 40 | - checkout_build_test: 41 | name: ros2-foxy-arm64 42 | resource_class: arm.medium 43 | arch: arm64v8 44 | ros_distro: foxy 45 | - checkout_build_test: 46 | name: ros2-galactic-arm64 47 | resource_class: arm.medium 48 | arch: arm64v8 49 | ros_distro: galactic 50 | - checkout_build_test: 51 | name: ros2-humble-arm64 52 | resource_class: arm.medium 53 | arch: arm64v8 54 | ros_distro: humble 55 | - checkout_build_test: 56 | name: ros2-iron-arm64 57 | resource_class: arm.medium 58 | arch: arm64v8 59 | ros_distro: iron 60 | - checkout_build_test: 61 | name: ros2-jazzy-arm64 62 | resource_class: arm.medium 63 | arch: arm64v8 64 | ros_distro: jazzy 65 | # See: https://github.com/orgs/foxglove/discussions/1127 66 | # - checkout_build_test: 67 | # name: ros2-kilted-arm64 68 | # resource_class: arm.medium 69 | # arch: arm64v8 70 | # ros_distro: kilted 71 | - checkout_build_test: 72 | name: ros2-rolling-arm64 73 | resource_class: arm.medium 74 | arch: arm64v8 75 | ros_distro: rolling 76 | - checkout_build_test: 77 | name: ros2-foxy-amd64 78 | resource_class: medium 79 | arch: amd64 80 | ros_distro: foxy 81 | - checkout_build_test: 82 | name: ros2-galactic-amd64 83 | resource_class: medium 84 | arch: amd64 85 | ros_distro: galactic 86 | - checkout_build_test: 87 | name: ros2-humble-amd64 88 | resource_class: medium 89 | arch: amd64 90 | ros_distro: humble 91 | - checkout_build_test: 92 | name: ros2-iron-amd64 93 | resource_class: medium 94 | arch: amd64 95 | ros_distro: iron 96 | - checkout_build_test: 97 | name: ros2-jazzy-amd64 98 | resource_class: medium 99 | arch: amd64 100 | ros_distro: jazzy 101 | # See: https://github.com/orgs/foxglove/discussions/1127 102 | # - checkout_build_test: 103 | # name: ros2-kilted-amd64 104 | # resource_class: medium 105 | # arch: amd64 106 | # ros_distro: kilted 107 | - checkout_build_test: 108 | name: ros2-rolling-amd64 109 | resource_class: medium 110 | arch: amd64 111 | ros_distro: rolling 112 | -------------------------------------------------------------------------------- /.devcontainer/foxy/devcontainer.json: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2025 Andrew Symington 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | { 23 | "build": { 24 | "dockerfile": "../../Dockerfile", 25 | "context": "../..", 26 | "args": { 27 | "ROS_DISTRO": "foxy" 28 | } 29 | }, 30 | "containerEnv": { 31 | "DISPLAY": "${localEnv:DISPLAY}", 32 | "QT_X11_NO_MITSHM": "1", 33 | "KRB5CCNAME": "${localEnv:KRB5CCNAME}" 34 | }, 35 | "extensions": [ 36 | "ms-python.python", 37 | "ms-vscode.cmake-tools", 38 | "ms-vscode.cpptools", 39 | "id:ms-azuretools.vscode-docker", 40 | "streetsidesoftware.code-spell-checker" 41 | ], 42 | "remoteUser": "ros", 43 | "containerUser": "ros", 44 | "forwardPorts": [8765, 9090], 45 | "workspaceMount": "source=${localWorkspaceFolder},target=/home/ros/ros2_ws/src/libsurvive_ros2,type=bind", 46 | "workspaceFolder": "/home/ros/ros2_ws" 47 | } 48 | -------------------------------------------------------------------------------- /.devcontainer/galactic/devcontainer.json: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2025 Andrew Symington 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | { 23 | "build": { 24 | "dockerfile": "../../Dockerfile", 25 | "context": "../..", 26 | "args": { 27 | "ROS_DISTRO": "galactic" 28 | } 29 | }, 30 | "containerEnv": { 31 | "DISPLAY": "${localEnv:DISPLAY}", 32 | "QT_X11_NO_MITSHM": "1", 33 | "KRB5CCNAME": "${localEnv:KRB5CCNAME}" 34 | }, 35 | "extensions": [ 36 | "ms-python.python", 37 | "ms-vscode.cmake-tools", 38 | "ms-vscode.cpptools", 39 | "id:ms-azuretools.vscode-docker", 40 | "streetsidesoftware.code-spell-checker" 41 | ], 42 | "remoteUser": "ros", 43 | "containerUser": "ros", 44 | "forwardPorts": [8765, 9090], 45 | "workspaceMount": "source=${localWorkspaceFolder},target=/home/ros/ros2_ws/src/libsurvive_ros2,type=bind", 46 | "workspaceFolder": "/home/ros/ros2_ws" 47 | } 48 | -------------------------------------------------------------------------------- /.devcontainer/humble/devcontainer.json: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2025 Andrew Symington 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | { 23 | "build": { 24 | "dockerfile": "../../Dockerfile", 25 | "context": "../..", 26 | "args": { 27 | "ROS_DISTRO": "humble" 28 | } 29 | }, 30 | "containerEnv": { 31 | "DISPLAY": "${localEnv:DISPLAY}", 32 | "QT_X11_NO_MITSHM": "1", 33 | "KRB5CCNAME": "${localEnv:KRB5CCNAME}" 34 | }, 35 | "extensions": [ 36 | "ms-python.python", 37 | "ms-vscode.cmake-tools", 38 | "ms-vscode.cpptools", 39 | "id:ms-azuretools.vscode-docker", 40 | "streetsidesoftware.code-spell-checker" 41 | ], 42 | "remoteUser": "ros", 43 | "containerUser": "ros", 44 | "forwardPorts": [8765, 9090], 45 | "workspaceMount": "source=${localWorkspaceFolder},target=/home/ros/ros2_ws/src/libsurvive_ros2,type=bind", 46 | "workspaceFolder": "/home/ros/ros2_ws" 47 | } 48 | -------------------------------------------------------------------------------- /.devcontainer/iron/devcontainer.json: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2025 Andrew Symington 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | { 23 | "build": { 24 | "dockerfile": "../../Dockerfile", 25 | "context": "../..", 26 | "args": { 27 | "ROS_DISTRO": "iron" 28 | } 29 | }, 30 | "containerEnv": { 31 | "DISPLAY": "${localEnv:DISPLAY}", 32 | "QT_X11_NO_MITSHM": "1", 33 | "KRB5CCNAME": "${localEnv:KRB5CCNAME}" 34 | }, 35 | "extensions": [ 36 | "ms-python.python", 37 | "ms-vscode.cmake-tools", 38 | "ms-vscode.cpptools", 39 | "id:ms-azuretools.vscode-docker", 40 | "streetsidesoftware.code-spell-checker" 41 | ], 42 | "remoteUser": "ros", 43 | "containerUser": "ros", 44 | "forwardPorts": [8765, 9090], 45 | "workspaceMount": "source=${localWorkspaceFolder},target=/home/ros/ros2_ws/src/libsurvive_ros2,type=bind", 46 | "workspaceFolder": "/home/ros/ros2_ws" 47 | } 48 | -------------------------------------------------------------------------------- /.devcontainer/jazzy/devcontainer.json: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2025 Andrew Symington 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | { 23 | "build": { 24 | "dockerfile": "../../Dockerfile", 25 | "context": "../..", 26 | "args": { 27 | "ROS_DISTRO": "jazzy" 28 | } 29 | }, 30 | "containerEnv": { 31 | "DISPLAY": "${localEnv:DISPLAY}", 32 | "QT_X11_NO_MITSHM": "1", 33 | "KRB5CCNAME": "${localEnv:KRB5CCNAME}" 34 | }, 35 | "extensions": [ 36 | "ms-python.python", 37 | "ms-vscode.cmake-tools", 38 | "ms-vscode.cpptools", 39 | "id:ms-azuretools.vscode-docker", 40 | "streetsidesoftware.code-spell-checker" 41 | ], 42 | "remoteUser": "ros", 43 | "containerUser": "ros", 44 | "forwardPorts": [8765, 9090], 45 | "workspaceMount": "source=${localWorkspaceFolder},target=/home/ros/ros2_ws/src/libsurvive_ros2,type=bind", 46 | "workspaceFolder": "/home/ros/ros2_ws" 47 | } 48 | -------------------------------------------------------------------------------- /.devcontainer/kilted/devcontainer.json: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2025 Andrew Symington 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | { 23 | "build": { 24 | "dockerfile": "../../Dockerfile", 25 | "context": "../..", 26 | "args": { 27 | "ROS_DISTRO": "kilted" 28 | } 29 | }, 30 | "containerEnv": { 31 | "DISPLAY": "${localEnv:DISPLAY}", 32 | "QT_X11_NO_MITSHM": "1", 33 | "KRB5CCNAME": "${localEnv:KRB5CCNAME}" 34 | }, 35 | "extensions": [ 36 | "ms-python.python", 37 | "ms-vscode.cmake-tools", 38 | "ms-vscode.cpptools", 39 | "id:ms-azuretools.vscode-docker", 40 | "streetsidesoftware.code-spell-checker" 41 | ], 42 | "remoteUser": "ros", 43 | "containerUser": "ros", 44 | "forwardPorts": [8765, 9090], 45 | "workspaceMount": "source=${localWorkspaceFolder},target=/home/ros/ros2_ws/src/libsurvive_ros2,type=bind", 46 | "workspaceFolder": "/home/ros/ros2_ws" 47 | } 48 | -------------------------------------------------------------------------------- /.devcontainer/rolling/devcontainer.json: -------------------------------------------------------------------------------- 1 | // MIT License 2 | // 3 | // Copyright (c) 2025 Andrew Symington 4 | // 5 | // Permission is hereby granted, free of charge, to any person obtaining a copy 6 | // of this software and associated documentation files (the "Software"), to deal 7 | // in the Software without restriction, including without limitation the rights 8 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | // copies of the Software, and to permit persons to whom the Software is 10 | // furnished to do so, subject to the following conditions: 11 | // 12 | // The above copyright notice and this permission notice shall be included in all 13 | // copies or substantial portions of the Software. 14 | // 15 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | // SOFTWARE. 22 | { 23 | "build": { 24 | "dockerfile": "../../Dockerfile", 25 | "context": "../..", 26 | "args": { 27 | "ROS_DISTRO": "rolling" 28 | } 29 | }, 30 | "containerEnv": { 31 | "DISPLAY": "${localEnv:DISPLAY}", 32 | "QT_X11_NO_MITSHM": "1", 33 | "KRB5CCNAME": "${localEnv:KRB5CCNAME}" 34 | }, 35 | "extensions": [ 36 | "ms-python.python", 37 | "ms-vscode.cmake-tools", 38 | "ms-vscode.cpptools", 39 | "id:ms-azuretools.vscode-docker", 40 | "streetsidesoftware.code-spell-checker" 41 | ], 42 | "remoteUser": "ros", 43 | "containerUser": "ros", 44 | "forwardPorts": [8765, 9090], 45 | "workspaceMount": "source=${localWorkspaceFolder},target=/home/ros/ros2_ws/src/libsurvive_ros2,type=bind", 46 | "workspaceFolder": "/home/ros/ros2_ws" 47 | } 48 | -------------------------------------------------------------------------------- /.github/FUNDING.yml: -------------------------------------------------------------------------------- 1 | # These are supported funding model platforms 2 | 3 | github: asymingt 4 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | .vscode -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Copyright 2022 Andrew Symington 2 | # 3 | # Permission is hereby granted, free of charge, to any person obtaining a copy 4 | # of this software and associated documentation files (the "Software"), to deal 5 | # in the Software without restriction, including without limitation the rights 6 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | # copies of the Software, and to permit persons to whom the Software is 8 | # furnished to do so, subject to the following conditions: 9 | # 10 | # The above copyright notice and this permission notice shall be included in 11 | # all copies or substantial portions of the Software. 12 | # 13 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 16 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | # THE SOFTWARE. 20 | 21 | # Declare the minimum cmake version 22 | cmake_minimum_required(VERSION 3.5) 23 | 24 | # Set the project name 25 | project(libsurvive_ros2) 26 | 27 | # Default to C99 28 | if(NOT CMAKE_C_STANDARD) 29 | set(CMAKE_C_STANDARD 99) 30 | endif() 31 | 32 | # Default to C++17 33 | if(NOT CMAKE_CXX_STANDARD) 34 | set(CMAKE_CXX_STANDARD 17) 35 | endif() 36 | 37 | # Set compile options 38 | add_compile_options(-Wall -Wextra -Wpedantic) 39 | 40 | # Find the core interface library 41 | include(ExternalProject) 42 | externalproject_add(libsurvive 43 | GIT_REPOSITORY https://github.com/cntools/libsurvive.git 44 | GIT_TAG 32cf62c52744fdc32003ef8169e8b81f6f31526b 45 | CMAKE_ARGS 46 | -DCMAKE_INSTALL_PREFIX=${CMAKE_CURRENT_BINARY_DIR}/libsurvive-install 47 | -DCMAKE_BUILD_TYPE=Release 48 | ) 49 | 50 | # Find dependencies 51 | find_package(ament_cmake REQUIRED) 52 | find_package(rclcpp REQUIRED) 53 | find_package(rclcpp_components REQUIRED) 54 | find_package(diagnostic_msgs REQUIRED) 55 | find_package(geometry_msgs REQUIRED) 56 | find_package(sensor_msgs REQUIRED) 57 | find_package(tf2 REQUIRED) 58 | find_package(tf2_ros REQUIRED) 59 | 60 | # Universally add this components includes 61 | include_directories(include) 62 | 63 | # Add the component 64 | add_library(libsurvive_ros2_component SHARED 65 | src/component.cpp) 66 | add_dependencies(libsurvive_ros2_component libsurvive) 67 | target_include_directories(libsurvive_ros2_component PUBLIC 68 | ${CMAKE_CURRENT_BINARY_DIR}/libsurvive-install/include 69 | ${CMAKE_CURRENT_BINARY_DIR}/libsurvive-install/include/libsurvive 70 | ${CMAKE_CURRENT_BINARY_DIR}/libsurvive-install/include/libsurvive/redist) 71 | target_link_directories(libsurvive_ros2_component PUBLIC 72 | ${CMAKE_CURRENT_BINARY_DIR}/libsurvive-install/lib) 73 | target_link_libraries(libsurvive_ros2_component -lsurvive) 74 | target_compile_definitions(libsurvive_ros2_component 75 | PRIVATE "COMPOSITION_BUILDING_DLL") 76 | ament_target_dependencies(libsurvive_ros2_component 77 | rclcpp 78 | rclcpp_components 79 | diagnostic_msgs 80 | geometry_msgs 81 | sensor_msgs 82 | tf2 83 | tf2_ros) 84 | rclcpp_components_register_nodes(libsurvive_ros2_component "libsurvive_ros2::Component") 85 | 86 | # Export the library path because the package installs libraries without exporting them 87 | if(NOT WIN32) 88 | ament_environment_hooks( 89 | "${ament_cmake_package_templates_ENVIRONMENT_HOOK_LIBRARY_PATH}") 90 | endif() 91 | 92 | # Manually compose the component into a node 93 | add_executable(libsurvive_ros2_node 94 | src/node.cpp) 95 | target_link_libraries(libsurvive_ros2_node 96 | libsurvive_ros2_component) 97 | ament_target_dependencies(libsurvive_ros2_node rclcpp) 98 | 99 | # Install libsurvive 100 | install( 101 | DIRECTORY 102 | ${CMAKE_CURRENT_BINARY_DIR}/libsurvive-install/ 103 | DESTINATION 104 | ${CMAKE_INSTALL_PREFIX} 105 | ) 106 | 107 | # Install the component 108 | install( 109 | TARGETS 110 | libsurvive_ros2_component 111 | ARCHIVE DESTINATION lib 112 | LIBRARY DESTINATION lib 113 | RUNTIME DESTINATION bin) 114 | 115 | # Install the node 116 | install( 117 | TARGETS 118 | libsurvive_ros2_node 119 | DESTINATION lib/${PROJECT_NAME}) 120 | 121 | # Install the shared things 122 | install( 123 | DIRECTORY 124 | config 125 | launch 126 | DESTINATION share/${PROJECT_NAME}) 127 | 128 | # For testing only 129 | if(BUILD_TESTING) 130 | find_package(ament_lint_auto REQUIRED) 131 | ament_lint_auto_find_test_dependencies() 132 | endif() 133 | 134 | # Export all include directories and declare the package 135 | ament_export_include_directories(include) 136 | ament_package() 137 | 138 | -------------------------------------------------------------------------------- /CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | Any contribution that you make to this repository will 2 | be under the MIT license, as dictated by that 3 | [license](https://opensource.org/licenses/MIT). -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2025 Andrew Symington 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | # 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | # 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | ARG ROS_DISTRO=humble 24 | ARG ARCH=amd64 25 | 26 | # Default image, user and root directory 27 | FROM ${ARCH}/ros:${ROS_DISTRO}-ros-base 28 | 29 | # Default user and group information 30 | ARG USERNAME=ros 31 | ARG USER_UID=1000 32 | ARG USER_GID=1000 33 | 34 | # Default shell to use in the RUN commands below. 35 | SHELL ["/bin/bash", "-c"] 36 | 37 | # Grab the latest ROS2 key to avoid an outdated key from affecting the build. 38 | # See: https://github.com/osrf/docker_images/issues/807. 39 | RUN rm -rf \ 40 | /etc/apt/sources.list.d/ros2.sources \ 41 | /etc/apt/sources.list.d/ros2-snapshots.list \ 42 | && apt-get --allow-unauthenticated --allow-insecure-repositories update \ 43 | && apt-get install -y --no-install-recommends curl \ 44 | && sudo rm -rf /var/lib/apt/lists/* \ 45 | && curl -sSL https://raw.githubusercontent.com/ros/rosdistro/master/ros.key \ 46 | -o /usr/share/keyrings/ros2-snapshots-archive-keyring.gpg \ 47 | && echo "deb [arch=$(dpkg --print-architecture) signed-by=/usr/share/keyrings/ros2-snapshots-archive-keyring.gpg] http://packages.ros.org/ros2/ubuntu $(. /etc/os-release && echo $UBUNTU_CODENAME) main" | tee /etc/apt/sources.list.d/ros2.list > /dev/null 48 | 49 | # Install baseline tools 50 | RUN apt-get update \ 51 | && apt-get install -y --no-install-recommends \ 52 | build-essential \ 53 | cmake \ 54 | freeglut3-dev \ 55 | gdb \ 56 | liblapacke-dev \ 57 | libopenblas-dev \ 58 | libpcap-dev \ 59 | libusb-1.0-0-dev \ 60 | libx11-dev \ 61 | sudo \ 62 | valgrind \ 63 | zlib1g-dev \ 64 | && sudo rm -rf /var/lib/apt/lists/* 65 | 66 | # Replace any existing 'ubuntu' user with a 'ros' user with specific privileges. 67 | RUN groupdel ubuntu || true \ 68 | && userdel -r ubuntu || true \ 69 | && groupadd --gid $USER_GID $USERNAME \ 70 | && useradd --uid $USER_UID --gid $USER_GID --shell /bin/bash -m $USERNAME \ 71 | && usermod -aG sudo,dialout,plugdev $USERNAME \ 72 | && echo '%sudo ALL=(ALL) NOPASSWD:ALL' >> /etc/sudoers 73 | 74 | # Switch to the non-root user. 75 | USER ros 76 | 77 | # Copy the source code into our test workspace. 78 | RUN mkdir -p /home/ros/.config /home/ros/ros2_ws/src/libsurvive_ros2 79 | COPY --chown=ros:ros . /home/ros/ros2_ws/src/libsurvive_ros2 80 | 81 | # Install baseline tools 82 | RUN sudo apt-get update \ 83 | && cd /home/ros/ros2_ws \ 84 | && rosdep update \ 85 | && rosdep install --from-paths src --ignore-src -r -y \ 86 | && source /opt/ros/$ROS_DISTRO/setup.bash \ 87 | && colcon build \ 88 | && sudo rm -rf /var/lib/apt/lists/* 89 | 90 | # Initialization 91 | RUN echo -e "#!/bin/bash \n\ 92 | set -e\n\ 93 | source /home/ros/ros2_ws/install/setup.bash \n\ 94 | exec \$@" > /home/ros/ros2_ws/entrypoint.sh 95 | RUN chmod 755 /home/ros/ros2_ws/entrypoint.sh 96 | WORKDIR /home/ros/ros2_ws 97 | ENTRYPOINT [ "/home/ros/ros2_ws/entrypoint.sh" ] 98 | 99 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Permission is hereby granted, free of charge, to any person obtaining a copy 2 | of this software and associated documentation files (the "Software"), to deal 3 | in the Software without restriction, including without limitation the rights 4 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 5 | copies of the Software, and to permit persons to whom the Software is 6 | furnished to do so, subject to the following conditions: 7 | 8 | The above copyright notice and this permission notice shall be included in 9 | all copies or substantial portions of the Software. 10 | 11 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 12 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 13 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 14 | THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 15 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 16 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 17 | THE SOFTWARE. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # libsurvive_ros2 2 | **A Steam-free driver for republishing Lighthouse 1.0 and 2.0 pose and sensor data to ROS2** 3 | 4 | [![CI Status](https://circleci.com/gh/asymingt/libsurvive_ros2.svg?style=svg)](https://app.circleci.com/pipelines/github/asymingt/libsurvive_ros2) 5 | 6 | This is a lightweight ROS2 wrapper around the [libsurvive](https://github.com/cntools/libsurvive) project, which provides a set of drivers for 6DoF rigid body tracking using SteamVR 1.0 and 2.0 hardware. It also listens for inertial, button, configuration and device connection events, and forwards these to various topics. 7 | 8 | This hardware is particularly useful to robotics projects, because it provides a cost effective method of obtaining ground truth with a positional accuracy typically in the sub-centimeter, sub-degree range. The final accuracy of course depends on the tracking volume, base station number and placement and level of occlusion, as well as calibration quality. 9 | 10 | The driver in this repo is largely based on the ROS1 driver [here](https://github.com/cntools/libsurvive/tree/master/tools/ros_publisher). It has been migrated to ROS2, and refactored slightly -- we use a thread to manage the blocking interaction with libsurvive, so that it doesn't lock the ROS2 callback queue and prevent messages from propagating correctly. 11 | 12 | When you build this code `cmake` will checkout and build the latest stable release of `libsurvive` and link against this library for you. This is to avoid having to discover it through pkg-config, and ensures that you are using a version that has been tested and is known to work well with ROS2. 13 | 14 | Progress: 15 | 16 | - [x] code ported 17 | - [x] documentation added 18 | - [x] foxglove example 19 | - [x] test imu callback 20 | - [x] test connect callback 21 | - [x] fix timestamp errors 22 | - [x] fix button callback (only works on Tracker 2.0 wirelessly via watchman) 23 | - [x] add linting checks (nice to have) 24 | - [ ] add unit tests 25 | - [ ] fix composable node (not a functional blocker right now) 26 | - [ ] convert to lifecycle node (nice to have) 27 | - [ ] add circlce, mergify and dependebot integration 28 | 29 | # Installation instructions 30 | 31 | This has only been tested on Ubuntu 22.04 and ROS Humble, although its fairly likely to work correctly with other distributions too. Pull requests are welcome if it does not! 32 | 33 | Before you do anything, you will need to install these udev rules and reload the subsystem. 34 | 35 | ``` 36 | sudo curl -fsSL https://raw.githubusercontent.com/cntools/libsurvive/master/useful_files/81-vive.rules \ 37 | -o /etc/udev/rules.d/81-vive.rules 38 | sudo udevadm control --reload-rules && udevadm trigger 39 | ``` 40 | 41 | You can now choose to build the driver natively or in a container. The benefit of launching it within a container is that it won't interfere with any pre-existing ROS installation on your machine. However, you will need docker-ce and the compose plugin for things to work. 42 | 43 | ## Devcontainer build and test (easiest for development) 44 | 45 | You can open this project as a [Visual Studio Code Devcontainer](https://code.visualstudio.com/docs/devcontainers/containers). It will then prompt you for a target ROS distribution in the drop-down menu at the top of the IDE. When you select your desired distribution, the devcontainer engine will build a container for this ROS distribution, install our project dependencies, and compile the code. You can just open up a terminal and run: 46 | 47 | ```sh 48 | ros@0d81683baf55:~/ros2_ws$ ros2 launch libsurvive_ros2 libsurvive_ros2.launch.py 49 | [INFO] [launch]: All log files can be found below /home/ros/.ros/log/2025-06-06-17-17-41-207302-0d81683baf55-2267 50 | [INFO] [launch]: Default logging verbosity is set to INFO 51 | [INFO] [libsurvive_ros2_node-1]: process started with pid [2304] 52 | [libsurvive_ros2_node-1] Info: Loaded drivers: GlobalSceneSolver, HTCVive 53 | [libsurvive_ros2_node-1] [INFO] [1749230261.309788197] [libsurvive.libsurvive_ros2_node]: Start listening for events.. 54 | [libsurvive_ros2_node-1] [INFO] [1749230261.309834518] [libsurvive.libsurvive_ros2_node]: Cleaning up. 55 | ``` 56 | 57 | Volume binding of the source tree happens with the correct permission. So, if you edit the source code in the container folder `/home/ros/ros2_ws/src/libsurvive_ros2` your changes will propagate to your host folder, where you can eventually push them upstream. It is also possible to use git within the container, but this is more complicated and not recommended. 58 | 59 | ## Containerized build and test (easiest and recommended) 60 | 61 | Install docker and docker-compose: https://docs.docker.com/engine/install/ubuntu/ 62 | 63 | ```sh 64 | $ docker compose build 65 | $ docker compose run libsurvive_ros2 colcon test 66 | ``` 67 | 68 | This will checkout a lightweight ROS2 rolling container, augment it with a few system dependencies, checkout and build the code and drop you into a bash shell as user `ros` at the home directory `/home/ros/ros2_ws/src`. If you'd rather build and test for ROS2 Foxy on arm64, as an example, you'd do this: 69 | 70 | ```sh 71 | $ docker compose build --build-arg ARCH=arm64 --build-arg ROS_DISTRO=foxy 72 | $ docker compose run libsurvive_ros2 colcon test # optional, to test the package 73 | ``` 74 | 75 | Note that if you are building on a different architecture than the host you must follow the docker/QEMU installation instructions here before running the command above. Here is a link to a document outlining how this is done: https://docs.nvidia.com/datacenter/cloud-native/playground/x-arch.html 76 | 77 | ## Native build and test (not recommended) 78 | 79 | You'll need ROS2 installed: https://docs.ros.org/en/humble/Installation.html 80 | 81 | You'll also need to follow the instructions here: 82 | 83 | ```sh 84 | sudo apt-get install build-essential \ 85 | cmake \ 86 | freeglut3-dev \ 87 | liblapacke-dev \ 88 | libopenblas-dev \ 89 | libpcap-dev \ 90 | libusb-1.0-0-dev \ 91 | libx11-dev \ 92 | zlib1g-dev 93 | ``` 94 | 95 | Finally source ROS2, create a workspace, checkout, compile and test: 96 | 97 | ```sh 98 | $ mkdir ~/ros2_ws/src 99 | $ cd ~/ros2_ws/src 100 | $ git clone https://github.com/asymingt/libsurvive_ros2.git 101 | $ cd .. 102 | $ rosdep update 103 | $ rosdep install --from-paths src --ignore-src -r -y 104 | $ colcon build 105 | $ colcon test # optional, to test the package 106 | $ source install/setup.bash 107 | ``` 108 | 109 | # Running the code 110 | 111 | To run the driver on containerized installations do the following: 112 | 113 | ```sh 114 | $ docker compose up 115 | ``` 116 | 117 | Alternatively, to run the driver on native installations run the following: 118 | 119 | ```sh 120 | $ ros2 launch libsurvive_ros2 libsurvive_ros2.launch.py rosbridge:=true 121 | ``` 122 | 123 | There are three launch arguments to `libsurvive_ros2.launch.py` to help get up and running: 124 | 125 | - `namespace = string (default: 'libsurvive')` : This is the namespace on which to add the extra topics for sensor data. 126 | - `record = boolean (default: false)` : Start a `ros2 bag record` to save `/tf` and `/tf_static` topics to a ros bag in the ROS2 log directory for the current launch ID as `libsurvive.bag`. 127 | - `foxbridge = boolean (default: false)` : Starts a `foxglove_bridge` to push data over a websocket at point 8765 (high performance). 128 | - `rosbridge = boolean (default: false)` : Starts a `rosbridge_server` and `rosapi` node to push data over a websocket at 9090 (more flexible). 129 | - `composable = boolean (default: false)`: For advanced users only -- it shows how to load the component-based version of the code to get zero-copy IPC between it and other composable nodes. 130 | 131 | # Example visualization with Foxglove 132 | 133 | After launch the stack (with `rosbridge:=true`), navigate to [this Foxglove link](https://studio.foxglove.dev/?ds=rosbridge-websocket&ds.url=ws%3A%2F%2Flocalhost%3A9090 134 | ) and you should see the data streaming: 135 | 136 | ![alt text](doc/foxglove.png) 137 | 138 | Now move the tracker around and you should see its corresponding transform move around inn the user interface. 139 | 140 | # Common questions 141 | 142 | - **I don't see any data streaming** Examine the console log. If you see a LIBUSB error, chances are high that you either haven't installed the udev rules, or you haven't mounted the /dev/bus/usb volume correctly into the docker container. 143 | 144 | - **How do I configure this for my specific tracker ID?** There's no need -- the libsurvive driver will enumerate all devices, query their ID and publish this ID as the transform name using the TF2 standard topic `/tf`. Base station positions change less frequently, and so they are published at a lowe rate on `/tf_static`. 145 | 146 | - **The base stations locations are not where I'd expect them to be** -- The calibration phase of libsurvive works out the relative location of the base stations. It has no idea of their orientation with respect to the room. To fix this, you will need to write your own static transform broadcaster to provide the relationship between your world frame and the `libsurvive_world` frame. 147 | 148 | - **In need to send extra arguments to the driver** -- Have a look at the `libsurvive_ros2.launch.py` file, and particularly at the `parameters` variable. You should probably be writing your own launch file, and you can include custom modifications for your specific tracking setup by changing the parameters you pass to the driver. 149 | 150 | 151 | 152 | -------------------------------------------------------------------------------- /config/config.json: -------------------------------------------------------------------------------- 1 | "v":"0", 2 | "poser":"MPFIT", 3 | "disambiguator":"StateBased", 4 | "configed-lighthouse-gen":"2", 5 | "ootx-ignore-sync-error":"0", 6 | "floor-offset":"-13.095158576965" 7 | "lighthouse0":{ 8 | "index":"0", 9 | "id":"384784632", 10 | "mode":"0", 11 | "pose":["-2.731781959534","-20.364048004150","-10.354103088379","-0.640586972523","-0.253050029391","-0.278953194743","-0.669177949728"], 12 | "variance":["0.493555217981","0.908196926117","0.793995380402","0.016432996839","0.017605012283","0.004369542468"], 13 | "unlock_count":"0", 14 | "accel":["-1.000000000000","125.000000000000","127.000000000000"], 15 | "fcalphase":["0.000000000000","-0.005214691162"], 16 | "fcaltilt":["-0.048187255859","0.045806884766"], 17 | "fcalcurve":["-0.022827148438","0.124389648438"], 18 | "fcalgibpha":["0.479248046875","0.283935546875"], 19 | "fcalgibmag":["0.002111434937","-0.002265930176"], 20 | "fcalogeephase":["1.112304687500","2.271484375000"], 21 | "fcalogeemag":["-0.391601562500","-0.382080078125"], 22 | "OOTXSet":"1", 23 | "PositionSet":"1" 24 | } 25 | "lighthouse1":{ 26 | "index":"1", 27 | "id":"1749379901", 28 | "mode":"1", 29 | "pose":["-7.594938278198","-19.912893295288","-9.974508285522","0.726724440392","0.175403191329","-0.168665705852","-0.642383987874"], 30 | "variance":["204.543045043945","1017.870483398438","650.074829101562","0.574005842209","0.190413609147","0.094315797091"], 31 | "unlock_count":"0", 32 | "accel":["6.000000000000","111.000000000000","127.000000000000"], 33 | "fcalphase":["0.000000000000","-0.003511428833"], 34 | "fcaltilt":["-0.049621582031","0.045410156250"], 35 | "fcalcurve":["0.049346923828","0.314208984375"], 36 | "fcalgibpha":["2.273437500000","2.234375000000"], 37 | "fcalgibmag":["-0.004455566406","-0.002807617188"], 38 | "fcalogeephase":["0.187988281250","1.401367187500"], 39 | "fcalogeemag":["-0.229248046875","-0.134887695312"], 40 | "OOTXSet":"1", 41 | "PositionSet":"1" 42 | } 43 | -------------------------------------------------------------------------------- /doc/foxglove.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/asymingt/libsurvive_ros2/13f44b310c33af304f3fe1e08858875f0822e10b/doc/foxglove.png -------------------------------------------------------------------------------- /docker-compose.yml: -------------------------------------------------------------------------------- 1 | # MIT License 2 | # 3 | # Copyright (c) 2025 Andrew Symington 4 | # 5 | # Permission is hereby granted, free of charge, to any person obtaining a copy 6 | # of this software and associated documentation files (the "Software"), to deal 7 | # in the Software without restriction, including without limitation the rights 8 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | # copies of the Software, and to permit persons to whom the Software is 10 | # furnished to do so, subject to the following conditions: 11 | # 12 | # The above copyright notice and this permission notice shall be included in all 13 | # copies or substantial portions of the Software. 14 | # 15 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | # AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | # SOFTWARE. 22 | 23 | # Docker Compose Spec Version 24 | # See: https://docs.docker.com/compose/compose-file/compose-versioning/#version-3 25 | 26 | services: 27 | libsurvive_ros2: 28 | build: . 29 | ports: 30 | - 8765 31 | - 9090 32 | entrypoint: /home/ros/ros2_ws/entrypoint.sh 33 | working_dir: /home/ros/ros2_ws 34 | command: ros2 launch libsurvive_ros2 libsurvive_ros2.launch.py foxbridge:=true composable:=false 35 | -------------------------------------------------------------------------------- /include/libsurvive_ros2/component.hpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Andrew Symington 2 | // 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | // 10 | // The above copyright notice and this permission notice shall be included in 11 | // all copies or substantial portions of the Software. 12 | // 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 16 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | // THE SOFTWARE. 20 | 21 | #ifndef LIBSURVIVE_ROS2__COMPONENT_HPP_ 22 | #define LIBSURVIVE_ROS2__COMPONENT_HPP_ 23 | 24 | #define SURVIVE_ENABLE_FULL_API 25 | 26 | // C system 27 | #include 28 | 29 | // C++ system 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | // Other 38 | #include "diagnostic_msgs/msg/key_value.hpp" 39 | #include "geometry_msgs/msg/point_stamped.hpp" 40 | #include "geometry_msgs/msg/pose_stamped.hpp" 41 | #include "libsurvive/survive_api.h" 42 | #include "libsurvive/survive.h" 43 | #include "rclcpp/rclcpp.hpp" 44 | #include "sensor_msgs/msg/imu.hpp" 45 | #include "sensor_msgs/msg/joy.hpp" 46 | #include "tf2_ros/static_transform_broadcaster.h" 47 | #include "tf2_ros/transform_broadcaster.h" 48 | 49 | namespace libsurvive_ros2 50 | { 51 | 52 | class Component : public rclcpp::Node 53 | { 54 | public: 55 | explicit Component(const rclcpp::NodeOptions & options); 56 | virtual ~Component(); 57 | rclcpp::Time get_ros_time(const std::string & str, FLT timecode); 58 | void publish_imu(const sensor_msgs::msg::Imu & msg); 59 | 60 | private: 61 | void work(); 62 | SurviveSimpleContext * actx_; 63 | std::unique_ptr tf_broadcaster_; 64 | std::shared_ptr tf_static_broadcaster_; 65 | rclcpp::Publisher::SharedPtr joy_publisher_; 66 | rclcpp::Publisher::SharedPtr imu_publisher_; 67 | rclcpp::Publisher::SharedPtr cfg_publisher_; 68 | std::thread worker_thread_; 69 | rclcpp::Time last_base_station_update_; 70 | std::string tracking_frame_; 71 | double lighthouse_rate_; 72 | }; 73 | 74 | } // namespace libsurvive_ros2 75 | 76 | #endif // LIBSURVIVE_ROS2__COMPONENT_HPP_ 77 | -------------------------------------------------------------------------------- /launch/libsurvive_ros2.launch.py: -------------------------------------------------------------------------------- 1 | # Copyright 2022 Andrew Symington 2 | # 3 | # Permission is hereby granted, free of charge, to any person obtaining a copy 4 | # of this software and associated documentation files (the "Software"), to deal 5 | # in the Software without restriction, including without limitation the rights 6 | # to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | # copies of the Software, and to permit persons to whom the Software is 8 | # furnished to do so, subject to the following conditions: 9 | # 10 | # The above copyright notice and this permission notice shall be included in 11 | # all copies or substantial portions of the Software. 12 | # 13 | # THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | # IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | # FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 16 | # THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | # LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | # OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | # THE SOFTWARE. 20 | 21 | import os 22 | 23 | from ament_index_python.packages import get_package_share_directory 24 | 25 | import launch 26 | from launch import LaunchDescription 27 | from launch.actions import DeclareLaunchArgument, ExecuteProcess 28 | from launch.conditions import IfCondition, UnlessCondition 29 | from launch.substitutions import LaunchConfiguration 30 | from launch_ros.actions import ComposableNodeContainer, Node 31 | from launch_ros.descriptions import ComposableNode 32 | 33 | # Bag to save data 34 | BAG_FILE = os.path.join(launch.logging.launch_config.log_dir, 'libsurvive.bag') 35 | 36 | # Default libsurvive configuration file 37 | CFG_FILE = os.path.join( 38 | get_package_share_directory('libsurvive_ros2'), 'config', 'config.json' 39 | ) 40 | 41 | # Sow we don't have to repeat for composable and non-composable versions. 42 | PARAMETERS = [ 43 | {'driver_args': f'--force-recalibrate 1 -c {CFG_FILE}'}, 44 | {'tracking_frame': 'libsurvive_world'}, 45 | {'imu_topic': 'imu'}, 46 | {'joy_topic': 'joy'}, 47 | {'cfg_topic': 'cfg'}, 48 | {'lighthouse_rate': 4.0}] 49 | 50 | 51 | def generate_launch_description(): 52 | arguments = [ 53 | DeclareLaunchArgument('namespace', default_value='libsurvive', 54 | description='Namespace for the non-TF topics'), 55 | DeclareLaunchArgument('composable', default_value='false', 56 | description='Launch in a composable container'), 57 | DeclareLaunchArgument('rosbridge', default_value='false', 58 | description='Launch a rosbridge'), 59 | DeclareLaunchArgument('foxbridge', default_value='false', 60 | description='Launch a foxglove bridge'), 61 | DeclareLaunchArgument('record', default_value='false', 62 | description='Record data with rosbag')] 63 | 64 | # Non-composable launch (regular node) 65 | libsurvive_node = Node( 66 | package='libsurvive_ros2', 67 | executable='libsurvive_ros2_node', 68 | name='libsurvive_ros2_node', 69 | namespace=LaunchConfiguration('namespace'), 70 | condition=UnlessCondition(LaunchConfiguration('composable')), 71 | output='screen', 72 | parameters=PARAMETERS) 73 | 74 | # Composable launch (zero-copy node example) 75 | libsurvive_composable_node = ComposableNodeContainer( 76 | package='rclcpp_components', 77 | executable='component_container', 78 | name='libsurvive_ros2_container', 79 | namespace=LaunchConfiguration('namespace'), 80 | condition=IfCondition(LaunchConfiguration('composable')), 81 | composable_node_descriptions=[ 82 | ComposableNode( 83 | package='libsurvive_ros2', 84 | plugin='libsurvive_ros2::Component', 85 | name='libsurvive_ros2_component', 86 | parameters=PARAMETERS, 87 | extra_arguments=[ 88 | {'use_intra_process_comms': True} 89 | ] 90 | ) 91 | ], 92 | output='log') 93 | 94 | # For ros webdocker bridge 95 | rosbridge_node = Node( 96 | package='rosbridge_server', 97 | executable='rosbridge_websocket', 98 | name='rosbridge_server_node', 99 | condition=IfCondition(LaunchConfiguration('rosbridge')), 100 | parameters=[ 101 | {'port': 9090}, 102 | ], 103 | output='log') 104 | rosapi_node = Node( 105 | package='rosapi', 106 | executable='rosapi_node', 107 | name='rosapi_node', 108 | condition=IfCondition(LaunchConfiguration('rosbridge')), 109 | output='log') 110 | 111 | # For foxglove websocket bridge. 112 | foxbridge_node = Node( 113 | package='foxglove_bridge', 114 | executable='foxglove_bridge', 115 | name='foxglove_bridge', 116 | condition=IfCondition(LaunchConfiguration('foxbridge')), 117 | parameters=[ 118 | {'port': 8765}, 119 | ], 120 | output='log') 121 | 122 | # For recording all data from the experiment 123 | bag_record_node = ExecuteProcess( 124 | cmd=['ros2', 'bag', 'record', '-o', BAG_FILE] + [ 125 | '/tf', 126 | '/tf_static' 127 | ], 128 | condition=IfCondition(LaunchConfiguration('record')), 129 | output='log') 130 | 131 | return LaunchDescription( 132 | arguments + [ 133 | libsurvive_node, 134 | libsurvive_composable_node, 135 | foxbridge_node, 136 | rosbridge_node, 137 | rosapi_node, 138 | bag_record_node 139 | ]) 140 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | libsurvive_ros2 6 | 7 | 0.0.1 8 | 9 | libsurvive driver for ROS2 10 | 11 | Andrew Symington 12 | 13 | MIT 14 | 15 | rclcpp 16 | rclcpp_components 17 | diagnostic_msgs 18 | geometry_msgs 19 | sensor_msgs 20 | tf2 21 | tf2_ros 22 | 23 | rosbridge_suite 24 | foxglove_bridge 25 | ament_lint_auto 26 | ament_lint_common 27 | 28 | 29 | ament_cmake 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /src/component.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Andrew Symington 2 | // 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | // 10 | // The above copyright notice and this permission notice shall be included in 11 | // all copies or substantial portions of the Software. 12 | // 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 16 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | // THE SOFTWARE. 20 | 21 | // C++ system 22 | #include 23 | #include 24 | #include 25 | 26 | // Other 27 | #include "libsurvive_ros2/component.hpp" 28 | #include "rclcpp_components/register_node_macro.hpp" 29 | 30 | 31 | // Scale factor to move from G to m/s^2. 32 | constexpr double SI_GRAVITY = 9.80665; 33 | 34 | // We can only ever load one version of the driver, so we store a pointer to the instance of the 35 | // driver here, so the IMU callback can push data to it. 36 | libsurvive_ros2::Component * _singleton = nullptr; 37 | 38 | static void imu_func( 39 | SurviveObject * so, int mask, const FLT * accelgyromag, uint32_t rawtime, int id) 40 | { 41 | if (_singleton) { 42 | survive_default_imu_process(so, mask, accelgyromag, rawtime, id); 43 | FLT timecode = SurviveSensorActivations_runtime( 44 | &so->activations, so->activations.last_imu) / FLT(1e6); 45 | sensor_msgs::msg::Imu imu_msg; 46 | imu_msg.header.frame_id = std::string(so->serial_number) + "_imu"; 47 | imu_msg.header.stamp = _singleton->get_ros_time("inertial", timecode); 48 | imu_msg.angular_velocity.x = accelgyromag[3]; 49 | imu_msg.angular_velocity.y = accelgyromag[4]; 50 | imu_msg.angular_velocity.z = accelgyromag[5]; 51 | imu_msg.linear_acceleration.x = accelgyromag[0] * SI_GRAVITY; 52 | imu_msg.linear_acceleration.y = accelgyromag[1] * SI_GRAVITY; 53 | imu_msg.linear_acceleration.z = accelgyromag[2] * SI_GRAVITY; 54 | _singleton->publish_imu(imu_msg); 55 | } 56 | } 57 | 58 | static void ros_from_pose( 59 | geometry_msgs::msg::Transform * const tx, const SurvivePose & pose) 60 | { 61 | tx->translation.x = pose.Pos[0]; 62 | tx->translation.y = pose.Pos[1]; 63 | tx->translation.z = pose.Pos[2]; 64 | tx->rotation.w = pose.Rot[0]; 65 | tx->rotation.x = pose.Rot[1]; 66 | tx->rotation.y = pose.Rot[2]; 67 | tx->rotation.z = pose.Rot[3]; 68 | } 69 | 70 | namespace libsurvive_ros2 71 | { 72 | 73 | Component::Component(const rclcpp::NodeOptions & options) 74 | : Node("libsurvive_ros2", options), 75 | actx_(nullptr), 76 | tf_broadcaster_(std::make_unique(*this)), 77 | tf_static_broadcaster_(std::make_unique(*this)) 78 | { 79 | // Store the instance globally to be used by a C callback. 80 | _singleton = this; 81 | 82 | // Global parameters 83 | this->declare_parameter("tracking_frame", "libsurvive_frame"); 84 | this->get_parameter("tracking_frame", tracking_frame_); 85 | this->declare_parameter("lighthouse_rate", 4.0); 86 | this->get_parameter("lighthouse_rate", lighthouse_rate_); 87 | 88 | // Setup topic for IMU. 89 | std::string imu_topic; 90 | this->declare_parameter("imu_topic", "imu"); 91 | this->get_parameter("imu_topic", imu_topic); 92 | imu_publisher_ = this->create_publisher(imu_topic, 10); 93 | 94 | // Setup topic for joystick. 95 | std::string joy_topic; 96 | this->declare_parameter("joy_topic", "joy"); 97 | this->get_parameter("joy_topic", joy_topic); 98 | joy_publisher_ = this->create_publisher(joy_topic, 10); 99 | 100 | // Setup topic for configuration. 101 | std::string cfg_topic; 102 | this->declare_parameter("cfg_topic", "cfg"); 103 | this->get_parameter("cfg_topic", cfg_topic); 104 | cfg_publisher_ = this->create_publisher(cfg_topic, 10); 105 | 106 | // Setup driver parameters. 107 | std::string driver_args; 108 | this->declare_parameter("driver_args", "--force-recalibrate 1"); 109 | this->get_parameter("driver_args", driver_args); 110 | std::vector args; 111 | std::stringstream driver_ss(driver_args); 112 | std::string token; 113 | while (getline(driver_ss, token, ' ')) { 114 | args.emplace_back(token.c_str()); 115 | } 116 | 117 | // Try and initialize survive with the arguments supplied. 118 | actx_ = survive_simple_init(args.size(), const_cast(args.data())); 119 | if (actx_ == nullptr) { 120 | RCLCPP_FATAL(this->get_logger(), "Could not initialize the libsurvive context"); 121 | return; 122 | } 123 | 124 | // Setup callback for reading IMU data. 125 | SurviveContext * ctx = survive_simple_get_ctx(actx_); 126 | survive_install_imu_fn(ctx, imu_func); 127 | 128 | // Initialize the survive thread. 129 | survive_simple_start_thread(actx_); 130 | 131 | // Start the work thread 132 | worker_thread_ = std::thread(&Component::work, this); 133 | } 134 | 135 | Component::~Component() 136 | { 137 | RCLCPP_INFO(this->get_logger(), "Cleaning up."); 138 | worker_thread_.join(); 139 | 140 | RCLCPP_INFO(this->get_logger(), "Shutting down libsurvive driver"); 141 | if (actx_) { 142 | survive_simple_close(actx_); 143 | } 144 | 145 | RCLCPP_INFO(this->get_logger(), "Clearing singleton instance"); 146 | _singleton = nullptr; 147 | } 148 | 149 | rclcpp::Time Component::get_ros_time(const std::string & /*str*/, FLT timecode) 150 | { 151 | return rclcpp::Time() + rclcpp::Duration(std::chrono::duration(timecode)); 152 | } 153 | 154 | void Component::publish_imu(const sensor_msgs::msg::Imu & msg) 155 | { 156 | if (imu_publisher_) { 157 | imu_publisher_->publish(msg); 158 | } 159 | } 160 | 161 | void Component::work() 162 | { 163 | RCLCPP_INFO(this->get_logger(), "Start listening for events.."); 164 | 165 | // Poll for events. 166 | struct SurviveSimpleEvent event = {}; 167 | while (survive_simple_wait_for_event( 168 | actx_, 169 | &event) != SurviveSimpleEventType_Shutdown && rclcpp::ok()) 170 | { 171 | // Business logic depends on the event type 172 | switch (event.event_type) { 173 | // TYPE: Pose update (limit to non-lighthouses only) 174 | case SurviveSimpleEventType_PoseUpdateEvent: { 175 | const struct SurviveSimplePoseUpdatedEvent * pose_event = 176 | survive_simple_get_pose_updated_event(&event); 177 | if (survive_simple_object_get_type(pose_event->object) != 178 | SurviveSimpleObject_LIGHTHOUSE) 179 | { 180 | SurvivePose pose = {}; 181 | auto timecode = survive_simple_object_get_latest_pose(pose_event->object, &pose); 182 | if (timecode > 0) { 183 | geometry_msgs::msg::TransformStamped pose_msg; 184 | pose_msg.header.stamp = this->get_ros_time("tracker", timecode); 185 | pose_msg.header.frame_id = tracking_frame_; 186 | pose_msg.child_frame_id = survive_simple_serial_number(pose_event->object); 187 | ros_from_pose(&pose_msg.transform, pose); 188 | tf_broadcaster_->sendTransform(pose_msg); 189 | } 190 | } 191 | break; 192 | } 193 | 194 | // TYPE: Button update 195 | case SurviveSimpleEventType_ButtonEvent: { 196 | const struct SurviveSimpleButtonEvent * button_event = survive_simple_get_button_event( 197 | &event); 198 | auto obj = button_event->object; 199 | sensor_msgs::msg::Joy joy_msg; 200 | joy_msg.header.frame_id = survive_simple_serial_number(button_event->object); 201 | joy_msg.header.stamp = this->get_ros_time("button", button_event->time); 202 | joy_msg.axes.resize(SURVIVE_MAX_AXIS_COUNT * 2); 203 | joy_msg.buttons.resize(SURVIVE_BUTTON_MAX * 2); 204 | int64_t mask = survive_simple_object_get_button_mask(obj); 205 | mask |= (survive_simple_object_get_touch_mask(obj) << SURVIVE_BUTTON_MAX); 206 | for (int i = 0; i < SURVIVE_MAX_AXIS_COUNT * 2; i++) { 207 | joy_msg.axes[i] = 208 | static_cast(survive_simple_object_get_input_axis(obj, (enum SurviveAxis)i)); 209 | } 210 | for (int i = 0; i < mask && i < static_cast(joy_msg.buttons.size()); i++) { 211 | joy_msg.buttons[i] = (mask >> i) & 1; 212 | } 213 | joy_publisher_->publish(joy_msg); 214 | break; 215 | } 216 | 217 | // TYPE: Configuration update 218 | case SurviveSimpleEventType_ConfigEvent: { 219 | const struct SurviveSimpleConfigEvent * config_event = survive_simple_get_config_event( 220 | &event); 221 | diagnostic_msgs::msg::KeyValue cfg_msg; 222 | cfg_msg.key = survive_simple_serial_number(config_event->object); 223 | cfg_msg.value = config_event->cfg; 224 | cfg_publisher_->publish(cfg_msg); 225 | break; 226 | } 227 | 228 | // TYPE: Device add event 229 | case SurviveSimpleEventType_DeviceAdded: { 230 | const struct SurviveSimpleObjectEvent * object_event = survive_simple_get_object_event( 231 | &event); 232 | RCLCPP_INFO( 233 | this->get_logger(), "A new device %s was added at time %lf", 234 | survive_simple_serial_number(object_event->object), 235 | this->get_ros_time("connect", object_event->time).seconds() 236 | ); 237 | break; 238 | } 239 | 240 | // TYPE: no-op 241 | case SurviveSimpleEventType_None: { 242 | break; 243 | } 244 | 245 | // We should never get here. 246 | default: 247 | RCLCPP_WARN(this->get_logger(), "Unknown event"); 248 | break; 249 | } 250 | 251 | // Always update the base stations 252 | auto time_now = this->get_clock()->now(); 253 | if (time_now.seconds() - last_base_station_update_.seconds() > lighthouse_rate_) { 254 | last_base_station_update_ = time_now; 255 | for (const SurviveSimpleObject * it = survive_simple_get_first_object(actx_); it != 0; 256 | it = survive_simple_get_next_object(actx_, it)) 257 | { 258 | if (survive_simple_object_get_type(it) == SurviveSimpleObject_LIGHTHOUSE) { 259 | SurvivePose pose = {}; 260 | auto timecode = survive_simple_object_get_latest_pose(it, &pose); 261 | if (timecode > 0) { 262 | geometry_msgs::msg::TransformStamped pose_msg; 263 | pose_msg.header.stamp = this->get_ros_time("lighthouse", timecode); 264 | pose_msg.header.frame_id = tracking_frame_; 265 | pose_msg.child_frame_id = survive_simple_serial_number(it); 266 | ros_from_pose(&pose_msg.transform, pose); 267 | tf_static_broadcaster_->sendTransform(pose_msg); 268 | } 269 | } 270 | } 271 | } 272 | } 273 | } 274 | 275 | } // namespace libsurvive_ros2 276 | 277 | // Register the component with class_loader. 278 | // This acts as a sort of entry point, allowing the component to be discoverable when its library 279 | // is being loaded into a running process. 280 | RCLCPP_COMPONENTS_REGISTER_NODE(libsurvive_ros2::Component) 281 | -------------------------------------------------------------------------------- /src/node.cpp: -------------------------------------------------------------------------------- 1 | // Copyright 2022 Andrew Symington 2 | // 3 | // Permission is hereby granted, free of charge, to any person obtaining a copy 4 | // of this software and associated documentation files (the "Software"), to deal 5 | // in the Software without restriction, including without limitation the rights 6 | // to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 7 | // copies of the Software, and to permit persons to whom the Software is 8 | // furnished to do so, subject to the following conditions: 9 | // 10 | // The above copyright notice and this permission notice shall be included in 11 | // all copies or substantial portions of the Software. 12 | // 13 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 14 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 15 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL 16 | // THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 17 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 18 | // OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 19 | // THE SOFTWARE. 20 | 21 | // C++ system 22 | #include 23 | 24 | // Others 25 | #include "libsurvive_ros2/component.hpp" 26 | #include "rclcpp/rclcpp.hpp" 27 | 28 | 29 | // Main entry point of application. 30 | int main(int argc, char * argv[]) 31 | { 32 | rclcpp::init(argc, argv); 33 | rclcpp::executors::SingleThreadedExecutor exec; 34 | exec.add_node(std::make_shared(rclcpp::NodeOptions{})); 35 | exec.spin(); 36 | rclcpp::shutdown(); 37 | return 0; 38 | } 39 | --------------------------------------------------------------------------------