|
| 1 | +## Setting up Hydra for a new sensor |
| 2 | + |
| 3 | +### Requirements |
| 4 | + |
| 5 | +This documentation is meant to be a quick guide to get you started with setting up a new launch file or configuration for Hydra. |
| 6 | +You will need: |
| 7 | +- An RGB-D camera |
| 8 | +- A source of odometry (i.e., something populating the pose of the camera for every image timestamp via TF) |
| 9 | +- A computer capable of running Nvidia TensorRT (i.e., an Nvidia GPU) |
| 10 | + |
| 11 | +> **Note** <br> |
| 12 | +> For the purposes of this guide (and in the code-base), we use the convention `parent_T_child` to represent the 6-DOF homogeneous transformation between from the `child` frame to the `parent` frame, or equivalently the pose of the `child` frame with respect to the `parent` frame. |
| 13 | +
|
| 14 | +Hydra does its best to follow [REP 105](https://www.ros.org/reps/rep-0105.html). There are four coordinate frames you should be familiar with and identify for your system: |
| 15 | +- **robot_frame**: This is typically `base_link` and serves the same purpose of REP 105; it is meant to be the root of the TF tree for the robot's body frame. |
| 16 | +- **sensor_frame**: This is the optical frame of the camera (i.e., x right, y down, z forward). |
| 17 | +- **odom_frame**: This is `odom` in REP 105. This serves as a reference frame for the robot and sensor poses (i.e., Hydra looks up `odom_T_robot` and `odom_T_sensor`). |
| 18 | +- **map_frame**: This is `map` in REP 105. Hydra can optionally broadcast `map_T_odom` as the relative transform between the last optimized and unoptimized robot pose. |
| 19 | + |
| 20 | +Finally, you will need to have set up `semantic_inference` following [these](https://github.com/MIT-SPARK/semantic_inference/blob/ros2/docs/closed_set.md) instructions, or have another 2D closed set dense semantic segmentation network capable of producing category labels. |
| 21 | + |
| 22 | +### Putting together the launch file |
| 23 | + |
| 24 | +We'll use the Zed2i camera as a working example. The camera provides the following topics: |
| 25 | +- `/zed/zed_node/left/image_rect_color`: Color image |
| 26 | +- `/zed/zed_node/left/camera_info`: Color camera information |
| 27 | +- `/zed/zed_node/depth/depth_registered`: Depth image (rectified to color camera) |
| 28 | +- `/zed/zed_node/depth/camera_info`: Depth camera information |
| 29 | +- ... |
| 30 | + |
| 31 | +#### Semantic Segmentation |
| 32 | + |
| 33 | +The first step is to set up the segmentation network by including `closed_set.launch.yaml` from `semantic_inference`: |
| 34 | +```yaml |
| 35 | +- group: # semantic segmentation |
| 36 | + - push_ros_namespace: zed |
| 37 | + - set_remap: {from: color/image_raw, to: zed_node/left/image_rect_color} |
| 38 | + - include: |
| 39 | + file: $(find-pkg-share semantic_inference_ros)/launch/closed_set.launch.yaml |
| 40 | + arg: |
| 41 | + - {name: labelspace_name, value: $(arg labelspace_name)} |
| 42 | +``` |
| 43 | +
|
| 44 | +There are three key points. |
| 45 | +
|
| 46 | +The first is that we namespace everything in the `zed` namespace to make connecting inputs to the node responsible for segmentation easier, and is good practice for multi-camera setups: |
| 47 | +```yaml |
| 48 | +- group: |
| 49 | + - push_ros_namespace: zed |
| 50 | +``` |
| 51 | + |
| 52 | +The second is that we remap the input of the closed set node: |
| 53 | +```yaml |
| 54 | + - set_remap: {from: color/image_raw, to: zed_node/left/image_rect_color} |
| 55 | +``` |
| 56 | + |
| 57 | +The third is that we need to set the labelspace that `semantic_inference` uses: |
| 58 | +```yaml |
| 59 | + arg: |
| 60 | + - {name: labelspace_name, value: $(arg labelspace_name)} |
| 61 | +``` |
| 62 | +This remaps the output of the network (the 150 categories from ade20k) to a condense labelspace of 50ish labels. |
| 63 | + |
| 64 | +#### Hydra |
| 65 | + |
| 66 | +Next, we add remappings to connect the input topics for Hydra to the camera: |
| 67 | +```yaml |
| 68 | +- set_remap: {from: hydra/input/camera/rgb/image_raw, to: /zed/zed_node/left/image_rect_color} |
| 69 | +- set_remap: {from: hydra/input/camera/rgb/camera_info, to: /zed/zed_node/left/camera_info} |
| 70 | +- set_remap: {from: hydra/input/camera/depth_registered/image_rect, to: /zed/zed_node/depth/depth_registered} |
| 71 | +- set_remap: {from: hydra/input/camera/semantic/image_raw, to: /zed/zed_node/semantic/image_raw} |
| 72 | +``` |
| 73 | + |
| 74 | +> **Note** <br> |
| 75 | +> We know the topics that Hydra expects for inputs based on the input name (`camera`) and receiver type (`ClosedSetImageReceiver`) that's specified in the input configuration (which we'll look at in more detail later). |
| 76 | + |
| 77 | +We then add Hydra: |
| 78 | +```yaml |
| 79 | +- node: # hydra |
| 80 | + pkg: hydra_ros |
| 81 | + exec: hydra_ros_node |
| 82 | + name: hydra |
| 83 | + args: > |
| 84 | + --config-utilities-yaml {map_frame: map, odom_frame: odom, robot_frame: zed_camera_link, sensor_frame: zed_left_camera_frame} |
| 85 | + --config-utilities-file $(find-pkg-share hydra_ros)/doc/examples/example_config.yaml |
| 86 | + --config-utilities-file $(find-pkg-share hydra)/config/label_spaces/$(var labelspace_name)_label_space.yaml |
| 87 | + --config-utilities-yaml {log_path: $(env HOME)/.hydra/$(var dataset)} |
| 88 | +``` |
| 89 | + |
| 90 | +> **Note** <br> |
| 91 | +> We specify configurations to Hydra with the command line features of `config_utilities`, which is documented [here](https://github.com/MIT-SPARK/config_utilities/blob/main/docs/Parsing.md#parse-from-the-command-line). |
| 92 | + |
| 93 | +The frame IDs that we identified earlier are specified here: |
| 94 | +```yaml |
| 95 | +{map_frame: map, odom_frame: odom, robot_frame: zed_camera_link, sensor_frame: zed_left_camera_frame} |
| 96 | +``` |
| 97 | + |
| 98 | +When Hydra shuts down, it will save the scene graph and various artifacts to |
| 99 | +```yaml |
| 100 | +{log_path: $(env HOME)/.hydra/$(var dataset)} |
| 101 | +``` |
| 102 | + |
| 103 | +#### Visualization |
| 104 | + |
| 105 | +Finally, we want to connect the output topic from Hydra (`hydra/backend/dsg`) to the input for the visualizer that renders the scene graph to RViz: |
| 106 | +```yaml |
| 107 | +- set_remap: {from: hydra_visualizer/dsg, to: hydra/backend/dsg} |
| 108 | +- include: # visualization |
| 109 | + file: $(find-pkg-share hydra_visualizer)/launch/streaming_visualizer.launch.yaml |
| 110 | +``` |
| 111 | + |
| 112 | +The full launch file can be found [here](./examples/example_camera.launch.yaml) |
| 113 | + |
| 114 | +### Configuring Hydra |
| 115 | + |
| 116 | +When making the launch file for Hydra, we included this line: |
| 117 | +```yaml |
| 118 | + --config-utilities-file $(find-pkg-share hydra_ros)/doc/examples/example_config.yaml |
| 119 | +``` |
| 120 | +which points Hydra to [this](./examples/example_config.yaml) file. |
| 121 | + |
| 122 | +This file is divided into several parts, which we'll also walk through. |
| 123 | + |
| 124 | +#### Configuring Inputs |
| 125 | + |
| 126 | +This block sets up the input(s) for Hydra: |
| 127 | +```yaml |
| 128 | +input: |
| 129 | + type: RosInput |
| 130 | + inputs: |
| 131 | + camera: |
| 132 | + receiver: |
| 133 | + type: ClosedSetImageReceiver |
| 134 | + queue_size: 30 |
| 135 | + sensor: |
| 136 | + type: camera_info |
| 137 | + min_range: 0.4 |
| 138 | + max_range: 5.0 |
| 139 | + extrinsics: |
| 140 | + type: ros |
| 141 | +``` |
| 142 | + |
| 143 | +In this case, we've specified a single closed set RGB-D camera (under the name `camera`), which reads intrinsic and extrinsic information from ROS (the intrinsics from a `sensor_msgs/msg/CameraInfo` message and the extrinsics from TF). |
| 144 | + |
| 145 | +#### Configuring the Active Window and Reconstruction |
| 146 | + |
| 147 | +This block sets up the size of the active window (an 8 meter spatial radius) and the resolution of the underlying voxel grid (10 centimeters): |
| 148 | +```yaml |
| 149 | +map_window: |
| 150 | + type: spatial |
| 151 | + max_radius_m: 8.0 |
| 152 | +active_window: |
| 153 | + volumetric_map: |
| 154 | + voxels_per_side: 16 |
| 155 | + voxel_size: 0.1 |
| 156 | + truncation_distance: 0.3 |
| 157 | + tsdf: |
| 158 | + semantic_integrator: |
| 159 | + type: MLESemanticIntegrator |
| 160 | +``` |
| 161 | + |
| 162 | +#### Other useful configuration points |
| 163 | + |
| 164 | +The backend of Hydra uses "update functors" to optimize and reconcile layers of the scene graph. |
| 165 | +These are specified in this block: |
| 166 | +```yaml |
| 167 | + update_functors: |
| 168 | + agents: {type: UpdateAgentsFunctor} |
| 169 | + objects: {type: UpdateObjectsFunctor} |
| 170 | + surface_places: {type: Update2dPlacesFunctor, min_size: 3} |
| 171 | +``` |
0 commit comments