diff --git a/.cmake-format.yaml b/.cmake-format.yaml index 4c92842..1653584 100644 --- a/.cmake-format.yaml +++ b/.cmake-format.yaml @@ -1,3 +1,4 @@ +--- parse: additional_commands: catkin_package: diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index a4e1c7f..7ad4631 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -1,7 +1,6 @@ --- -# based on https://github.com/actions/starter-workflows/blob/main/ci/cmake-single-platform.yml name: Hydra-ROS Build and Test -on: {push: {branches: [main]}, pull_request: {branches: [main]}} +on: {push: {branches: [main, develop]}, pull_request: {branches: [main, develop]}} jobs: ros2: runs-on: ubuntu-latest @@ -19,7 +18,7 @@ jobs: - name: Dependencies run: | apt update && apt install -y python3-vcstool git ccache - vcs import src < src/hydra_ros/install/packages.yaml + vcs import src < src/hydra_ros/install/ci/${{ github.base_ref || github.ref_name }}.yaml rosdep update --rosdistro jazzy && rosdep install --rosdistro jazzy --from-paths src --ignore-src -r -y - name: Build shell: bash diff --git a/.github/workflows/linting.yaml b/.github/workflows/linting.yaml index ecbd4a9..094f288 100644 --- a/.github/workflows/linting.yaml +++ b/.github/workflows/linting.yaml @@ -1,6 +1,6 @@ --- # based on https://github.com/actions/starter-workflows/blob/main/ci/cmake-single-platform.yml -name: Hydra-ROS Linting +name: Linting on: {push: {branches: [main, develop]}, pull_request: {}} jobs: lint: diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 8c97ee6..1823cdc 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -1,16 +1,26 @@ --- # See https://pre-commit.com for more information # See https://pre-commit.com/hooks.html for more hooks -exclude: .*\.tsv|\.clang-format +exclude: .*\.tsv|\.clang-format|.*docker-compose.yml repos: - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v3.2.0 + rev: v6.0.0 hooks: - id: trailing-whitespace - id: end-of-file-fixer - - id: check-yaml - id: check-added-large-files - repo: https://github.com/pre-commit/mirrors-clang-format - rev: v14.0.6 + rev: v21.1.8 hooks: - id: clang-format + - repo: https://github.com/astral-sh/ruff-pre-commit + rev: v0.14.14 + hooks: + - {id: ruff, args: [--select, I, --fix]} + - id: ruff + - id: ruff-format + - repo: https://github.com/lyz-code/yamlfix + rev: 1.19.1 + hooks: + - id: yamlfix + entry: env YAMLFIX_LINE_LENGTH=200 yamlfix diff --git a/README.md b/README.md index 3e02a7c..1393d00 100644 --- a/README.md +++ b/README.md @@ -74,7 +74,7 @@ echo "build: {cmake-args: [-DCMAKE_BUILD_TYPE=Release]}" > colcon_defaults.yaml cd src git clone git@github.com:MIT-SPARK/Hydra-ROS.git hydra_ros -vcs import . < hydra_ros/install/ros2.yaml +vcs import . < hydra_ros/install/packages.yaml rosdep install --from-paths . --ignore-src -r -y cd .. diff --git a/dev/check-code.sh b/dev/check-code.sh deleted file mode 100755 index 5e637fd..0000000 --- a/dev/check-code.sh +++ /dev/null @@ -1,8 +0,0 @@ -#!/bin/bash - -SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd ) -pushd $(dirname $SCRIPT_DIR) - -find ./python -iname '*.py' | xargs python3 -m flake8 - -popd diff --git a/dev/check-licenses.sh b/dev/check-licenses.sh deleted file mode 100755 index b162539..0000000 --- a/dev/check-licenses.sh +++ /dev/null @@ -1,10 +0,0 @@ -#!/bin/bash - -SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd ) - -pushd $(dirname $SCRIPT_DIR) - -find . -iname *.h -o -iname *.cpp | xargs python3 $SCRIPT_DIR/check_license.py -find . -iname '*.py' | xargs python3 $SCRIPT_DIR/check_license.py - -popd diff --git a/dev/check_license.py b/dev/check_license.py deleted file mode 100644 index 91f2492..0000000 --- a/dev/check_license.py +++ /dev/null @@ -1,73 +0,0 @@ -#!/usr/bin/env python3 -"""Script to check source file for license info.""" -import difflib -import pathlib -import click - - -def get_filetype(extension): - """Get filetype based on extension.""" - if extension in [".cpp", ".h"]: - return "cpp" - elif extension == ".py": - return "python" - else: - return None - - -@click.command() -@click.argument("input_files", type=click.File("r"), nargs=-1) -@click.option("--verbose", "-v", is_flag=True, help="show files checked") -def main(input_files, verbose): - """Check if header of file matches license.""" - parent_dir = pathlib.Path(__file__).absolute().parent - headers = {} - with (parent_dir / "cpp-header.txt").open("r") as fin: - headers["cpp"] = fin.read() - - with (parent_dir / "python-header.txt").open("r") as fin: - headers["python"] = fin.read() - - for input_file in input_files: - ext = pathlib.Path(input_file.name).suffix - filetype = get_filetype(ext) - if not filetype: - click.secho(f"unknown extension for {input_file.name}: {ext}", fg="yellow") - continue - - target = headers[filetype] - num_lines = len(target.split("\n")) - 1 - - try: - lines = [next(input_file) for _ in range(num_lines)] - except StopIteration: - click.secho(f"{input_file.name}: missing", fg="red") - continue - - if filetype == "python": - if lines and "#!" in lines[0]: - lines = lines[1:] - try: - lines.append(next(input_file)) - except StopIteration: - click.secho(f"{input_file.name}: missing", fg="red") - continue - - header = "".join(lines) - if header != target: - click.secho(f"{input_file.name}: bad", fg="red") - if verbose: - diff = difflib.context_diff( - header.splitlines(keepends=True), - target.splitlines(keepends=True), - fromfile=input_file.name, - tofile="license", - ) - diff_str = "".join(diff) - click.secho(f"{diff_str}", fg="red") - elif verbose: - click.secho(f"{input_file.name}: good", fg="green") - - -if __name__ == "__main__": - main() diff --git a/dev/cpp-header.txt b/dev/cpp-header.txt deleted file mode 100644 index 254cf9a..0000000 --- a/dev/cpp-header.txt +++ /dev/null @@ -1,34 +0,0 @@ -/* ----------------------------------------------------------------------------- - * Copyright 2022 Massachusetts Institute of Technology. - * All Rights Reserved - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions are met: - * - * 1. Redistributions of source code must retain the above copyright notice, - * this list of conditions and the following disclaimer. - * - * 2. 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. - * - * 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 HOLDER 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. - * - * Research was sponsored by the United States Air Force Research Laboratory and - * the United States Air Force Artificial Intelligence Accelerator and was - * accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views - * and conclusions contained in this document are those of the authors and should - * not be interpreted as representing the official policies, either expressed or - * implied, of the United States Air Force or the U.S. Government. The U.S. - * Government is authorized to reproduce and distribute reprints for Government - * purposes notwithstanding any copyright notation herein. - * -------------------------------------------------------------------------- */ diff --git a/dev/python-header.txt b/dev/python-header.txt deleted file mode 100644 index 38e6d5c..0000000 --- a/dev/python-header.txt +++ /dev/null @@ -1,32 +0,0 @@ -# Copyright 2022, Massachusetts Institute of Technology. -# All Rights Reserved -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# 1. Redistributions of source code must retain the above copyright notice, -# this list of conditions and the following disclaimer. -# -# 2. 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. -# -# 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 HOLDER 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. -# -# Research was sponsored by the United States Air Force Research Laboratory and -# the United States Air Force Artificial Intelligence Accelerator and was -# accomplished under Cooperative Agreement Number FA8750-19-2-1000. The views -# and conclusions contained in this document are those of the authors and should -# not be interpreted as representing the official policies, either expressed or -# implied, of the United States Air Force or the U.S. Government. The U.S. -# Government is authorized to reproduce and distribute reprints for Government -# purposes notwithstanding any copyright notation herein. diff --git a/dev/reformat.sh b/dev/reformat.sh deleted file mode 100755 index abdae9f..0000000 --- a/dev/reformat.sh +++ /dev/null @@ -1,11 +0,0 @@ -#!/bin/bash - -SCRIPT_DIR=$( cd -- "$( dirname -- "${BASH_SOURCE[0]}" )" &> /dev/null && pwd ) - -pushd $(dirname $SCRIPT_DIR) - -find . -iname *.h -o -iname *.cpp | xargs clang-format -i -python3 -m black . -find . -iname *CMakeLists.txt* | xargs python3 -m cmakelang.format -i - -popd diff --git a/docker/README.md b/docker/README.md index 11dca3f..ddb8367 100644 --- a/docker/README.md +++ b/docker/README.md @@ -60,9 +60,7 @@ Before using Docker, make sure to: cd src git clone https://github.com/MIT-SPARK/Hydra-ROS.git hydra_ros - - #replace ros2.yaml with ros2_docker.yaml to use https - vcs import . < hydra_ros/install/ros2.yaml + vcs import . < hydra_ros/install/packages.yaml ``` > :warning: **Warning**
@@ -131,7 +129,7 @@ cd $WORKSPACE/src git clone https://github.com/stereolabs/zed-ros2-wrapper.git ``` -2. While the uhumans2 dataset has pre-segmented images, you must run semantic segmentation on the images. You can use [semantic_inference](https://github.com/MIT-SPARK/semantic_inference), which should already be installed via `ros2.yaml`. Download the default [pretrained model](https://drive.google.com/file/d/1XRcsyLSvqqhqNIaOI_vmqpUpmBT6gk9-/view?usp=drive_link) to the directory `$WORKSPACE/.models/`. +2. While the uhumans2 dataset has pre-segmented images, you must run semantic segmentation on the images. You can use [semantic_inference](https://github.com/MIT-SPARK/semantic_inference), which should already be installed via `packages.yaml`. Download the default [pretrained model](https://drive.google.com/file/d/1XRcsyLSvqqhqNIaOI_vmqpUpmBT6gk9-/view?usp=drive_link) to the directory `$WORKSPACE/.models/`. 3. (optional) To avoid re-optimizing the model when running the container, set the `ZED_CACHE` environment variable to mount a host directory for the zed cache: diff --git a/hydra_ros/app/csv_to_tf b/hydra_ros/app/csv_to_tf index 2ce5a52..6f8954a 100755 --- a/hydra_ros/app/csv_to_tf +++ b/hydra_ros/app/csv_to_tf @@ -4,12 +4,11 @@ import pathlib import sys +import geometry_msgs.msg import rclpy import rclpy.node import rclpy.utilities import tf2_ros -import geometry_msgs.msg - DEFAULT_ORDER = ["x", "y", "z", "qw", "qx", "qy", "qz"] @@ -58,7 +57,9 @@ class CsvToTf(rclpy.node.Node): self._stale_threshold = rclpy.duration.Duration(seconds=stale_threshold_s) lookahead_s = self._get_param("lookahead_s", 0.1).double_value self._lookahead = rclpy.duration.Duration(seconds=lookahead_s) - self.get_logger().info(f"offset: {self._offset}, lookahead: {self._lookahead}, stale: {self._stale_threshold}") + self.get_logger().info( + f"offset: {self._offset}, lookahead: {self._lookahead}, stale: {self._stale_threshold}" + ) trajectory_file = self._get_param("trajectory_file", "").string_value if trajectory_file == "": diff --git a/hydra_ros/app/dsg_republisher b/hydra_ros/app/dsg_republisher index b4315a8..2abcced 100755 --- a/hydra_ros/app/dsg_republisher +++ b/hydra_ros/app/dsg_republisher @@ -1,9 +1,11 @@ #!/usr/bin/env python3 """Script that receives and republishes a dsg.""" -import hydra_ros + import rclpy from rclpy.node import Node +import hydra_ros + class DsgRepublishNode(Node): """Node to republish a DSG.""" diff --git a/hydra_ros/app/noisy_tf_publisher b/hydra_ros/app/noisy_tf_publisher index 282fb38..91d258a 100755 --- a/hydra_ros/app/noisy_tf_publisher +++ b/hydra_ros/app/noisy_tf_publisher @@ -2,11 +2,10 @@ import numpy as np import rclpy -from rclpy.node import Node - import tf2_ros from geometry_msgs.msg import TransformStamped from nav_msgs.msg import Odometry +from rclpy.node import Node from scipy.spatial.transform import Rotation as R diff --git a/hydra_ros/app/odom_to_tf b/hydra_ros/app/odom_to_tf index e637d5e..6a23358 100755 --- a/hydra_ros/app/odom_to_tf +++ b/hydra_ros/app/odom_to_tf @@ -1,11 +1,11 @@ #!/usr/bin/env python3 """Broadcast a tf from odom.""" +import geometry_msgs.msg +import nav_msgs.msg import rclpy -from rclpy.node import Node import tf2_ros -import nav_msgs.msg -import geometry_msgs.msg +from rclpy.node import Node class OdomToTf(Node): diff --git a/hydra_ros/config/datasets/a1.yaml b/hydra_ros/config/datasets/a1.yaml index 52645c3..ef396b2 100644 --- a/hydra_ros/config/datasets/a1.yaml +++ b/hydra_ros/config/datasets/a1.yaml @@ -1,3 +1,4 @@ +--- map_frame: map odom_frame: cam_t265_odom_frame robot_frame: cam_d455_link diff --git a/hydra_ros/config/datasets/kitti_360.yaml b/hydra_ros/config/datasets/kitti_360.yaml index 99e51b1..db7fb6f 100644 --- a/hydra_ros/config/datasets/kitti_360.yaml +++ b/hydra_ros/config/datasets/kitti_360.yaml @@ -12,7 +12,7 @@ input: min_range: $(arg sensor_min_range) max_range: $(arg sensor_max_range) is_asymmetric: true - horizontal_resolution: 0.16 # true resolution 0.08 + horizontal_resolution: 0.16 # true resolution 0.08 vertical_resolution: 0.41875 # true resolution 0.41875 horizontal_fov: 360.0 vertical_fov: 26.8 diff --git a/hydra_ros/config/datasets/zed2i.yaml b/hydra_ros/config/datasets/zed2i.yaml index 2c0e7fc..48fd340 100644 --- a/hydra_ros/config/datasets/zed2i.yaml +++ b/hydra_ros/config/datasets/zed2i.yaml @@ -1,3 +1,4 @@ +--- map_frame: map odom_frame: odom robot_frame: zed_camera_link diff --git a/hydra_ros/config/sinks/mesh_segmenter_sinks.yaml b/hydra_ros/config/sinks/mesh_segmenter_sinks.yaml index c116d80..a034ee3 100644 --- a/hydra_ros/config/sinks/mesh_segmenter_sinks.yaml +++ b/hydra_ros/config/sinks/mesh_segmenter_sinks.yaml @@ -1,2 +1,3 @@ +--- sinks: - type: ObjectVisualizer diff --git a/hydra_ros/hydra_ros/__init__.py b/hydra_ros/hydra_ros/__init__.py index 1b9f015..ce7c815 100644 --- a/hydra_ros/hydra_ros/__init__.py +++ b/hydra_ros/hydra_ros/__init__.py @@ -1,14 +1,16 @@ """ROS2 Python Package for Hydra-ROS.""" from typing import Optional + import rclpy -from rclpy.node import Node import spark_dsg as dsg -from hydra_msgs.msg import DsgUpdate -from std_msgs.msg import Header from builtin_interfaces.msg import Time +from rclpy.node import Node +from rclpy.qos import QoSDurabilityPolicy, QoSHistoryPolicy, QoSProfile +from std_msgs.msg import Header + +from hydra_msgs.msg import DsgUpdate -from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSHistoryPolicy class DsgPublisher: """Class for publishing a scene graph from python.""" @@ -20,16 +22,20 @@ def __init__(self, node: Node, topic: str, publish_mesh: bool = True): qos_profile = QoSProfile( history=QoSHistoryPolicy.KEEP_ALL, depth=10, - durability=QoSDurabilityPolicy.TRANSIENT_LOCAL + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, ) self._pub = node.create_publisher(DsgUpdate, topic, qos_profile) - def publish(self, G, stamp: Optional[rclpy.time.Time] = None, frame_id: str = "odom"): + def publish( + self, G, stamp: Optional[rclpy.time.Time] = None, frame_id: str = "odom" + ): """Send a graph.""" now = stamp if stamp is not None else rclpy.clock.Clock().now() header = Header() - header.stamp = Time(sec=now.nanoseconds // 10**9, nanosec=now.nanoseconds % 10**9) + header.stamp = Time( + sec=now.nanoseconds // 10**9, nanosec=now.nanoseconds % 10**9 + ) header.frame_id = frame_id self.publish_with_header(G, header) @@ -45,7 +51,7 @@ def publish_with_header(self, G, header: Header): class DsgSubscriber: """Class for receiving a scene graph in python.""" - def __init__(self, node: Node, topic: str, callback, qos = None): + def __init__(self, node: Node, topic: str, callback, qos=None): """Construct a DSG Receiver.""" self._callback = callback self._graph_set = False @@ -54,10 +60,7 @@ def __init__(self, node: Node, topic: str, callback, qos = None): qos_profile = qos or QoSProfile(depth=1) self._sub = node.create_subscription( - DsgUpdate, - topic, - self._handle_update, - qos_profile + DsgUpdate, topic, self._handle_update, qos_profile ) def _handle_update(self, msg: DsgUpdate): @@ -65,12 +68,12 @@ def _handle_update(self, msg: DsgUpdate): raise NotImplementedError("Partial updates not implemented yet") size_bytes = len(msg.layer_contents) - self._logger.debug( - f"Received dsg update message of {size_bytes} bytes" - ) + self._logger.debug(f"Received dsg update message of {size_bytes} bytes") if not self._graph_set: - self._graph = dsg.DynamicSceneGraph.from_binary(msg.layer_contents.tobytes()) + self._graph = dsg.DynamicSceneGraph.from_binary( + msg.layer_contents.tobytes() + ) self._graph_set = True else: self._graph.update_from_binary(msg.layer_contents.tobytes()) diff --git a/hydra_ros/launch/datasets/a1.launch.yaml b/hydra_ros/launch/datasets/a1.launch.yaml index 77721bd..54122ed 100644 --- a/hydra_ros/launch/datasets/a1.launch.yaml +++ b/hydra_ros/launch/datasets/a1.launch.yaml @@ -1,162 +1,150 @@ +--- launch: - - arg: { name: model_name, default: "/root/hydra_ws/.models/ade20k-efficientvit_seg_l2.onnx" } - - arg: { name: model_config_path, default: "/root/hydra_ws/src/semantic_inference/semantic_inference_ros/config/models/ade20k-efficientvit_seg_l2.yaml" } - - arg: { name: labelspace_name, default: "ade20k_mit" } - - arg: { name: use_sim_time, default: "false" } - - arg: { name: dataset, default: "a1" } - - - arg: { name: rgb_compressed_topic, default: "/cam_d455/color/image_raw/compressed" } - - arg: { name: rgb_topic, default: "/cam_d455/color/image_raw" } - - arg: { name: camera_info_topic, default: "/cam_d455/color/camera_info" } - - arg: { name: depth_topic_raw, default: "/cam_d455/depth/image_rect_raw" } - - arg: { name: depth_topic, default: "/depth_registered/image_rect" } - - arg: { name: depth_info_topic, default: "/cam_d455/depth/camera_info" } - - #TODO(jared): currently, these don't do anything - - arg: { name: enable_decompress, default: "false" } - - arg: { name: enable_register_depth, default: "false" } - - arg: { name: enable_pc, default: "false" } - - #extrinsics + - arg: {name: model_name, default: /root/hydra_ws/.models/ade20k-efficientvit_seg_l2.onnx} + - arg: {name: model_config_path, default: /root/hydra_ws/src/semantic_inference/semantic_inference_ros/config/models/ade20k-efficientvit_seg_l2.yaml} + - arg: {name: labelspace_name, default: ade20k_mit} + - arg: {name: use_sim_time, default: 'false'} + - arg: {name: dataset, default: a1} + - arg: {name: rgb_compressed_topic, default: /cam_d455/color/image_raw/compressed} + - arg: {name: rgb_topic, default: /cam_d455/color/image_raw} + - arg: {name: camera_info_topic, default: /cam_d455/color/camera_info} + - arg: {name: depth_topic_raw, default: /cam_d455/depth/image_rect_raw} + - arg: {name: depth_topic, default: /depth_registered/image_rect} + - arg: {name: depth_info_topic, default: /cam_d455/depth/camera_info} + + # TODO(jared): currently, these don't do anything + - arg: {name: enable_decompress, default: 'false'} + - arg: {name: enable_register_depth, default: 'false'} + - arg: {name: enable_pc, default: 'false'} + + # extrinsics - node: pkg: tf2_ros exec: static_transform_publisher name: map_to_odom - args: "0 0 0 0 0 0 1 map cam_t265_odom_frame" + args: 0 0 0 0 0 0 1 map cam_t265_odom_frame - #result from kalibr --- difficult to connect to tf tree, so we use - #t265_to_d455_bridge below, which was computed backward through - #the static tf tree e.g., T^IMU_CAM = T^IMU_OPT * T^OPT_CAM + # result from kalibr --- difficult to connect to tf tree, so we use + # t265_to_d455_bridge below, which was computed backward through + # the static tf tree e.g., T^IMU_CAM = T^IMU_OPT * T^OPT_CAM # - node: # pkg: tf2_ros # exec: static_transform_publisher # name: cam_t265_imu_optical_frame_to_cam_d455_color_optical_frame # args: "0.02216565 0.01795623 -0.21828447 0.9999320589420063 -0.008628001195529594 0.004334249473494838 0.006530518690349069 cam_t265_imu_optical_frame cam_d455_color_optical_frame_bridge" - - node: pkg: tf2_ros exec: static_transform_publisher name: t265_to_d455_bridge - args: "-0.03682085814765222 0.018967817042455707 -0.21903849135413533 -0.502094165524598 0.5043749875316702 0.48921230299321244 0.504161326857448 cam_t265_imu_optical_frame cam_d455_link" + args: -0.03682085814765222 0.018967817042455707 -0.21903849135413533 -0.502094165524598 0.5043749875316702 0.48921230299321244 0.504161326857448 cam_t265_imu_optical_frame cam_d455_link - #extracted tfs (t265) + # extracted tfs (t265) - node: pkg: tf2_ros exec: static_transform_publisher name: t265_static_tf_0 - args: "0.00009436560503672808 0.021378569304943085 -0.00007563530380139127 2.220446049250313e-16 0.0 1.0 0.0 cam_t265_link cam_t265_gyro_frame" - + args: 0.00009436560503672808 0.021378569304943085 -0.00007563530380139127 2.220446049250313e-16 0.0 1.0 0.0 cam_t265_link cam_t265_gyro_frame - node: pkg: tf2_ros exec: static_transform_publisher name: t265_static_tf_1 - args: "0 0 0 0.5 -0.4999999999999999 -0.5 0.5000000000000001 cam_t265_gyro_frame cam_t265_imu_optical_frame" - + args: 0 0 0 0.5 -0.4999999999999999 -0.5 0.5000000000000001 cam_t265_gyro_frame cam_t265_imu_optical_frame - node: pkg: tf2_ros exec: static_transform_publisher name: t265_static_tf_2 - args: "0.00009436560503672808 0.021378569304943085 -0.00007563530380139127 2.220446049250313e-16 0.0 1.0 0.0 cam_t265_link cam_t265_accel_frame" - + args: 0.00009436560503672808 0.021378569304943085 -0.00007563530380139127 2.220446049250313e-16 0.0 1.0 0.0 cam_t265_link cam_t265_accel_frame - node: pkg: tf2_ros exec: static_transform_publisher name: t265_static_tf_3 - args: "0 0 0 0.5 -0.4999999999999999 -0.5 0.5000000000000001 cam_t265_accel_frame cam_t265_accel_optical_frame" - + args: 0 0 0 0.5 -0.4999999999999999 -0.5 0.5000000000000001 cam_t265_accel_frame cam_t265_accel_optical_frame - node: pkg: tf2_ros exec: static_transform_publisher name: t265_static_tf_4 - args: "0 0 0 0 0 0 1 cam_t265_pose_frame cam_t265_link" + args: 0 0 0 0 0 0 1 cam_t265_pose_frame cam_t265_link - #extracted tfs (d455) + # extracted tfs (d455) - node: pkg: tf2_ros exec: static_transform_publisher name: d455_static_tf_0 - args: "0.0 -0.0 -0.0 0.0 0.0 0.0 1.0 cam_d455_link cam_d455_depth_frame" - + args: 0.0 -0.0 -0.0 0.0 0.0 0.0 1.0 cam_d455_link cam_d455_depth_frame - node: pkg: tf2_ros exec: static_transform_publisher name: d455_static_tf_1 - args: "0.0 -0.0 -0.0 -0.5 0.4999999999999999 -0.5 0.5000000000000001 cam_d455_depth_frame cam_d455_depth_optical_frame" - + args: 0.0 -0.0 -0.0 -0.5 0.4999999999999999 -0.5 0.5000000000000001 cam_d455_depth_frame cam_d455_depth_optical_frame - node: pkg: tf2_ros exec: static_transform_publisher name: d455_static_tf_2 - args: "-0.0002813368628267199 -0.059279922395944595 -3.9372611354338005e-05 4.3593594455160065e-05 0.00020654933177866042 0.002206892240792513 0.9999975562095642 cam_d455_link cam_d455_color_frame" - + args: -0.0002813368628267199 -0.059279922395944595 -3.9372611354338005e-05 4.3593594455160065e-05 0.00020654933177866042 0.002206892240792513 0.9999975562095642 cam_d455_link cam_d455_color_frame - node: pkg: tf2_ros exec: static_transform_publisher name: d455_static_tf_3 - args: "0.0 -0.0 -0.0 -0.5 0.4999999999999999 -0.5 0.5000000000000001 cam_d455_color_frame cam_d455_color_optical_frame" - + args: 0.0 -0.0 -0.0 -0.5 0.4999999999999999 -0.5 0.5000000000000001 cam_d455_color_frame cam_d455_color_optical_frame - node: pkg: tf2_ros exec: static_transform_publisher name: d455_static_tf_4 - args: "-0.016019999980926514 -0.030220000073313713 0.007400000002235174 0.0 0.0 0.0 1.0 cam_d455_link cam_d455_gyro_frame" - + args: -0.016019999980926514 -0.030220000073313713 0.007400000002235174 0.0 0.0 0.0 1.0 cam_d455_link cam_d455_gyro_frame - node: pkg: tf2_ros exec: static_transform_publisher name: d455_static_tf_5 - args: "0.0 -0.0 -0.0 -0.5 0.4999999999999999 -0.5 0.5000000000000001 cam_d455_gyro_frame cam_d455_imu_optical_frame" - + args: 0.0 -0.0 -0.0 -0.5 0.4999999999999999 -0.5 0.5000000000000001 cam_d455_gyro_frame cam_d455_imu_optical_frame - node: pkg: tf2_ros exec: static_transform_publisher name: d455_static_tf_6 - args: "-0.016019999980926514 -0.030220000073313713 0.007400000002235174 0.0 0.0 0.0 1.0 cam_d455_link cam_d455_accel_frame" - + args: -0.016019999980926514 -0.030220000073313713 0.007400000002235174 0.0 0.0 0.0 1.0 cam_d455_link cam_d455_accel_frame - node: pkg: tf2_ros exec: static_transform_publisher name: d455_static_tf_7 - args: "0.0 -0.0 -0.0 -0.5 0.4999999999999999 -0.5 0.5000000000000001 cam_d455_accel_frame cam_d455_accel_optical_frame" + args: 0.0 -0.0 -0.0 -0.5 0.4999999999999999 -0.5 0.5000000000000001 cam_d455_accel_frame cam_d455_accel_optical_frame - #decompress images - #todo(jared): add condition + # decompress images + # todo(jared): add condition - node: pkg: image_transport exec: republish name: decompress_rgb param: - - { name: in_transport, value: "compressed" } - - { name: out_transport, value: "raw" } + - {name: in_transport, value: compressed} + - {name: out_transport, value: raw} remap: - - { from: in/compressed, to: $(var rgb_compressed_topic) } - - { from: out, to: $(var rgb_topic) } + - {from: in/compressed, to: $(var rgb_compressed_topic)} + - {from: out, to: $(var rgb_topic)} - #register depth - #todo(jared): add condition + # register depth + # todo(jared): add condition - node: pkg: depth_image_proc exec: register_node name: depth_register remap: - - { from: depth/image_rect, to: $(var depth_topic_raw) } - - { from: depth/camera_info, to: $(var depth_info_topic) } - - { from: rgb/camera_info, to: $(var camera_info_topic) } + - {from: depth/image_rect, to: $(var depth_topic_raw)} + - {from: depth/camera_info, to: $(var depth_info_topic)} + - {from: rgb/camera_info, to: $(var camera_info_topic)} param: - name: queue_size value: 10 - #publish point cloud from color + depth (only for visualization) - #todo(jared): add condition + # publish point cloud from color + depth (only for visualization) + # todo(jared): add condition - node: pkg: depth_image_proc exec: point_cloud_xyzrgb_node name: depth_to_pointcloud remap: - - { from: depth/image_rect, to: $(var depth_topic) } - - { from: depth/camera_info, to: $(var depth_info_topic) } - - { from: rgb/image_rect_color, to: $(var rgb_topic) } - - { from: rgb/camera_info, to: $(var camera_info_topic) } + - {from: depth/image_rect, to: $(var depth_topic)} + - {from: depth/camera_info, to: $(var depth_info_topic)} + - {from: rgb/image_rect_color, to: $(var rgb_topic)} + - {from: rgb/camera_info, to: $(var camera_info_topic)} param: - name: queue_size value: 10 @@ -168,9 +156,9 @@ launch: namespace: frontleft name: segmentation remap: - - { from: color/image_raw, to: $(var rgb_topic) } - - { from: color/camera_info, to: $(var camera_info_topic) } - - { from: depth/image_rect, to: $(var depth_topic) } + - {from: color/image_raw, to: $(var rgb_topic)} + - {from: color/camera_info, to: $(var camera_info_topic)} + - {from: depth/image_rect, to: $(var depth_topic)} args: > --config-utilities-file $(var model_config_path) --config-utilities-file $(find-pkg-share semantic_inference_ros)/config/label_groupings/$(var labelspace_name).yaml@output/recolor @@ -178,11 +166,11 @@ launch: --config-utilities-yaml {segmenter: {model: {model_file: $(var model_name)}}} --config-utilities-yaml {worker: {max_queue_size: 1, image_separation_s: 0.0}} - #hydra - - set_remap: { from: hydra/input/frontleft/rgb/image_raw, to: $(var rgb_topic) } - - set_remap: { from: hydra/input/frontleft/depth_registered/image_rect, to: $(var depth_topic) } - - set_remap: { from: hydra/input/frontleft/rgb/camera_info, to: $(var camera_info_topic) } - - set_remap: { from: hydra/input/frontleft/semantic/image_raw, to: /frontleft/semantic/image_raw } + # hydra + - set_remap: {from: hydra/input/frontleft/rgb/image_raw, to: $(var rgb_topic)} + - set_remap: {from: hydra/input/frontleft/depth_registered/image_rect, to: $(var depth_topic)} + - set_remap: {from: hydra/input/frontleft/rgb/camera_info, to: $(var camera_info_topic)} + - set_remap: {from: hydra/input/frontleft/semantic/image_raw, to: /frontleft/semantic/image_raw} - node: pkg: hydra_ros exec: hydra_ros_node @@ -195,7 +183,6 @@ launch: --config-utilities-yaml {print_missing: false, show_config: true, verbosity: 2} --config-utilities-yaml {log_path: $(env HOME)/.hydra/$(var dataset)} --config-utilities-yaml {glog_level: 0, glog_verbosity: 0} - - set_remap: {from: hydra_visualizer/dsg, to: hydra/backend/dsg} - include: file: $(find-pkg-share hydra_visualizer)/launch/streaming_visualizer.launch.yaml diff --git a/hydra_ros/launch/hydra.launch.yaml b/hydra_ros/launch/hydra.launch.yaml index 450da65..11bf965 100644 --- a/hydra_ros/launch/hydra.launch.yaml +++ b/hydra_ros/launch/hydra.launch.yaml @@ -26,7 +26,7 @@ launch: - arg: {name: log_path, default: $(env HOME)/.hydra/$(var log_name), description: Directory to output to} - arg: {name: exit_after_clock, default: 'false', description: Shutdown hydra node after a rosbag finishes playing} - arg: {name: enable_zmq, default: 'false', description: Turn on zmq-based publishing for scene graph} - - arg: {name: zmq_url, default: 'tcp://127.0.0.1:8001', description: ZMQ publish url} + - arg: {name: zmq_url, default: tcp://127.0.0.1:8001, description: ZMQ publish url} - arg: {name: zmq_send_mesh, default: 'false', description: Whether or not to serialize mesh when using ZMQ} # hydra node - node: @@ -52,7 +52,8 @@ launch: --config-utilities-yaml {enable_zmq: $(var enable_zmq), backend: {zmq_sink: {url: $(var zmq_url), send_mesh: $(var zmq_send_mesh)}}} --config-utilities-yaml $(var extra_yaml) # hydra visualizer - - set_remap: {from: hydra_visualizer/dsg, to: hydra/backend/dsg} + - arg: {name: graph_to_visualize, default: backend} + - set_remap: {from: hydra_visualizer/dsg, to: hydra/$(var graph_to_visualize)/dsg} - arg: {name: start_visualizer, default: 'true', description: run hydra visualizer} - include: file: $(find-pkg-share hydra_visualizer)/launch/streaming_visualizer.launch.yaml diff --git a/hydra_ros/scripts/dump_bag_odom.py b/hydra_ros/scripts/dump_bag_odom.py index c520db8..b79ef3d 100755 --- a/hydra_ros/scripts/dump_bag_odom.py +++ b/hydra_ros/scripts/dump_bag_odom.py @@ -34,9 +34,11 @@ # # """Copy static tfs from one uhumans2 bag to a launch file.""" -import rosbag + import argparse +import rosbag + LINE_STR = "{ts},{x},{y},{z},{qw},{qx},{qy},{qz}\n" diff --git a/hydra_visualizer/config/visualizer_config.yaml b/hydra_visualizer/config/visualizer_config.yaml index b3ba352..35571f9 100644 --- a/hydra_visualizer/config/visualizer_config.yaml +++ b/hydra_visualizer/config/visualizer_config.yaml @@ -1,29 +1,23 @@ --- renderer: - layer_z_step: 5.5 # unit separation between layers - collapse_layers: false # whether or not to apply offsets to each of the layers + layer_z_step: 5.5 # unit separation between layers + collapse_layers: false # whether or not to apply offsets to each of the layers layers: 2: z_offset_scale: 2.0 visualize: true - nodes: { scale: 0.40, color: { type: LabelColorAdapter }, alpha: 0.8, use_sphere: false } - text: { draw: true, collapse: true, adapter: { type: LabelTextAdapter }, height: 0.5, scale: 0.45 } - bounding_boxes: { draw: true, collapse: true, scale: 0.05, edge_scale: 0.05, alpha: 0.9, edge_break_ratio: 0.5 } - edges: { interlayer_use_source: true, interlayer_scale: 0.08, interlayer_alpha: 0.9 } + nodes: {scale: 0.40, color: {type: LabelColorAdapter}, alpha: 0.8, use_sphere: false} + text: {draw: true, collapse: true, adapter: {type: LabelTextAdapter}, height: 0.5, scale: 0.45} + bounding_boxes: {draw: true, collapse: true, scale: 0.05, edge_scale: 0.05, alpha: 0.9, edge_break_ratio: 0.5} + edges: {interlayer_use_source: true, interlayer_scale: 0.08, interlayer_alpha: 0.9} 3: z_offset_scale: 3.0 visualize: true - nodes: - { - scale: 0.2, - color: { type: ParentColorAdapter, colormap: { palette: colorbrewer } }, - alpha: 0.9, - use_sphere: true, - } + nodes: {scale: 0.2, color: {type: ParentColorAdapter, colormap: {palette: colorbrewer}}, alpha: 0.9, use_sphere: true} edges: scale: 0.01 alpha: 0.5 - color: { type: UniformEdgeColorAdapter } + color: {type: UniformEdgeColorAdapter} interlayer_use_source: false interlayer_scale: 0.08 interlayer_alpha: 0.4 @@ -31,18 +25,12 @@ renderer: 4: z_offset_scale: 4.2 visualize: true - nodes: - { - scale: 0.6, - color: { type: IdColorAdapter, colormap: { palette: colorbrewer } }, - alpha: 0.8, - use_sphere: false, - } - text: { draw: true, height: 1.25, scale: 1.0 } + nodes: {scale: 0.6, color: {type: IdColorAdapter, colormap: {palette: colorbrewer}}, alpha: 0.8, use_sphere: false} + text: {draw: true, height: 1.25, scale: 1.0} edges: scale: 0.1 alpha: 0.2 - color: { type: UniformEdgeColorAdapter } + color: {type: UniformEdgeColorAdapter} interlayer_use_source: true interlayer_scale: 0.08 interlayer_alpha: 0.4 @@ -51,18 +39,18 @@ renderer: 2: z_offset_scale: 0.0 visualize: true - nodes: { scale: 0.15, alpha: 0.9, use_sphere: false, color: { type: PartitionColorAdapter } } - edges: { scale: 0.05, alpha: 0.9, draw_interlayer: false } - text: { draw_layer: true, height: 0.9, scale: 0.8 } + nodes: {scale: 0.15, alpha: 0.9, use_sphere: false, color: {type: PartitionColorAdapter}} + edges: {scale: 0.05, alpha: 0.9, draw_interlayer: false} + text: {draw_layer: true, height: 0.9, scale: 0.8} 3: z_offset_scale: 3.0 visualize: true - nodes: { scale: 0.2, color: { type: LabelColorAdapter }, alpha: 0.9, use_sphere: true } - boundaries: { draw: true, collapse: false, wireframe_scale: 0.1, use_node_color: true, alpha: 1.0 } + nodes: {scale: 0.2, color: {type: LabelColorAdapter}, alpha: 0.9, use_sphere: true} + boundaries: {draw: true, collapse: false, wireframe_scale: 0.1, use_node_color: true, alpha: 1.0} edges: scale: 0.01 alpha: 0.5 - color: { type: UniformEdgeColorAdapter } + color: {type: UniformEdgeColorAdapter} interlayer_use_source: false interlayer_scale: 0.08 interlayer_alpha: 0.4 diff --git a/hydra_visualizer/config/visualizer_plugins.yaml b/hydra_visualizer/config/visualizer_plugins.yaml index 886b728..5980d44 100644 --- a/hydra_visualizer/config/visualizer_plugins.yaml +++ b/hydra_visualizer/config/visualizer_plugins.yaml @@ -12,7 +12,7 @@ plugins: places_node_scale: 0.1 khronos_objects: type: KhronosObjectPlugin - dynamic_color_mode: CONSTANT # ID, SEMANTIC, CONSTANT + dynamic_color_mode: CONSTANT # ID, SEMANTIC, CONSTANT places_traversability: type: TraversabilityPlugin agent_poses: diff --git a/hydra_visualizer/include/hydra_visualizer/adapters/graph_color.h b/hydra_visualizer/include/hydra_visualizer/adapters/graph_color.h index b0b206f..57e19be 100644 --- a/hydra_visualizer/include/hydra_visualizer/adapters/graph_color.h +++ b/hydra_visualizer/include/hydra_visualizer/adapters/graph_color.h @@ -168,13 +168,6 @@ struct IsActiveFunctor : StatusFunctor { config::Registration("is_active"); }; -struct NeedsCleanupFunctor : StatusFunctor { - virtual bool eval(const spark_dsg::DynamicSceneGraph& graph, - const spark_dsg::SceneGraphNode& node) const override; - inline static const auto registration = - config::Registration("needs_cleanup"); -}; - struct HasActiveMeshFunctor : StatusFunctor { virtual bool eval(const spark_dsg::DynamicSceneGraph& graph, const spark_dsg::SceneGraphNode& node) const override; diff --git a/hydra_visualizer/launch/streaming_visualizer.launch.yaml b/hydra_visualizer/launch/streaming_visualizer.launch.yaml index 36a56bc..b9acaa2 100644 --- a/hydra_visualizer/launch/streaming_visualizer.launch.yaml +++ b/hydra_visualizer/launch/streaming_visualizer.launch.yaml @@ -12,7 +12,7 @@ launch: # communication - arg: {name: use_zmq, default: 'false', description: use zmq to receive scene graphs} - arg: {name: visualizer_frame, default: map, description: frame ID for zmq to use} - - arg: {name: zmq_url, default: 'tcp://127.0.0.1:8001', description: full zmq url} + - arg: {name: zmq_url, default: tcp://127.0.0.1:8001, description: full zmq url} # visualizer node and control for launching - arg: {name: start_visualizer, default: 'true'} - node: diff --git a/hydra_visualizer/src/adapters/graph_color.cpp b/hydra_visualizer/src/adapters/graph_color.cpp index 0aeed14..6773e3a 100644 --- a/hydra_visualizer/src/adapters/graph_color.cpp +++ b/hydra_visualizer/src/adapters/graph_color.cpp @@ -137,11 +137,6 @@ bool IsActiveFunctor::eval(const DynamicSceneGraph&, const SceneGraphNode& node) return node.attributes().is_active; } -bool NeedsCleanupFunctor::eval(const DynamicSceneGraph&, - const SceneGraphNode& node) const { - return node.attributes().need_cleanup_splitting; -} - bool HasActiveMeshFunctor::eval(const DynamicSceneGraph&, const SceneGraphNode& node) const { return node.attributes().has_active_mesh_indices; diff --git a/hydra_visualizer/src/drawing.cpp b/hydra_visualizer/src/drawing.cpp index 7e9f21e..6176b27 100644 --- a/hydra_visualizer/src/drawing.cpp +++ b/hydra_visualizer/src/drawing.cpp @@ -551,7 +551,7 @@ Marker makeMeshEdgesMarker(const std_msgs::msg::Header& header, for (const auto& [node_id, node] : layer.nodes()) { const auto& attrs = node->attributes(); - const auto& mesh_edge_indices = attrs.pcl_mesh_connections; + const auto& mesh_edge_indices = attrs.mesh_connections; if (mesh_edge_indices.empty()) { continue; } diff --git a/install/ci/develop.yaml b/install/ci/develop.yaml new file mode 100644 index 0000000..5538286 --- /dev/null +++ b/install/ci/develop.yaml @@ -0,0 +1,42 @@ +--- +repositories: + config_utilities: + type: git + url: https://github.com/MIT-SPARK/config_utilities.git + version: main + hydra: + type: git + url: https://github.com/MIT-SPARK/Hydra.git + version: develop + ianvs: + type: git + url: https://github.com/MIT-SPARK/Ianvs.git + version: main + kimera_pgmo: + type: git + url: https://github.com/MIT-SPARK/Kimera-PGMO.git + version: ros2 + kimera_rpgo: + type: git + url: https://github.com/MIT-SPARK/Kimera-RPGO.git + version: develop + pose_graph_tools: + type: git + url: https://github.com/MIT-SPARK/pose_graph_tools.git + version: main + semantic_inference: + type: git + url: https://github.com/MIT-SPARK/semantic_inference.git + version: main + spark_dsg: + type: git + url: https://github.com/MIT-SPARK/Spark-DSG.git + version: develop + spatial_hash: + type: git + url: https://github.com/MIT-SPARK/Spatial-Hash.git + version: main + teaser_plusplus: + type: git + url: https://github.com/MIT-SPARK/TEASER-plusplus.git + version: master diff --git a/install/ros2_docker.yaml b/install/ci/main.yaml similarity index 91% rename from install/ros2_docker.yaml rename to install/ci/main.yaml index d806938..f862224 100644 --- a/install/ros2_docker.yaml +++ b/install/ci/main.yaml @@ -1,3 +1,4 @@ +--- repositories: config_utilities: type: git @@ -7,10 +8,6 @@ repositories: type: git url: https://github.com/MIT-SPARK/Hydra.git version: main - hydra_ros: - type: git - url: https://github.com/MIT-SPARK/Hydra-ROS.git - version: main ianvs: type: git url: https://github.com/MIT-SPARK/Ianvs.git @@ -26,7 +23,7 @@ repositories: pose_graph_tools: type: git url: https://github.com/MIT-SPARK/pose_graph_tools.git - version: ros2 + version: main semantic_inference: type: git url: https://github.com/MIT-SPARK/semantic_inference.git diff --git a/install/packages.yaml b/install/packages.yaml index fdee4cd..7c55583 100644 --- a/install/packages.yaml +++ b/install/packages.yaml @@ -1,3 +1,4 @@ +--- repositories: config_utilities: type: git @@ -7,6 +8,10 @@ repositories: type: git url: https://github.com/MIT-SPARK/Hydra.git version: main + hydra_ros: + type: git + url: https://github.com/MIT-SPARK/Hydra-ROS.git + version: main ianvs: type: git url: https://github.com/MIT-SPARK/Ianvs.git @@ -22,7 +27,7 @@ repositories: pose_graph_tools: type: git url: https://github.com/MIT-SPARK/pose_graph_tools.git - version: ros2 + version: main semantic_inference: type: git url: https://github.com/MIT-SPARK/semantic_inference.git diff --git a/install/ros2.yaml b/install/ros2.yaml deleted file mode 100644 index da99565..0000000 --- a/install/ros2.yaml +++ /dev/null @@ -1,45 +0,0 @@ -repositories: - config_utilities: - type: git - url: git@github.com:MIT-SPARK/config_utilities.git - version: main - hydra: - type: git - url: git@github.com:MIT-SPARK/Hydra.git - version: main - hydra_ros: - type: git - url: git@github.com:MIT-SPARK/Hydra-ROS.git - version: main - ianvs: - type: git - url: git@github.com:MIT-SPARK/Ianvs.git - version: main - kimera_pgmo: - type: git - url: git@github.com:MIT-SPARK/Kimera-PGMO.git - version: ros2 - kimera_rpgo: - type: git - url: git@github.com:MIT-SPARK/Kimera-RPGO.git - version: develop - pose_graph_tools: - type: git - url: git@github.com:MIT-SPARK/pose_graph_tools.git - version: ros2 - semantic_inference: - type: git - url: git@github.com:MIT-SPARK/semantic_inference.git - version: main - spark_dsg: - type: git - url: git@github.com:MIT-SPARK/Spark-DSG.git - version: main - spatial_hash: - type: git - url: git@github.com:MIT-SPARK/Spatial-Hash.git - version: main - teaser_plusplus: - type: git - url: git@github.com:MIT-SPARK/TEASER-plusplus.git - version: master