Get Hydra working with Bosch Stretch

hydra_ros/config/color/bosch_office.csv

@@ -0,0 +1,151 @@
+name,red,green,blue,alpha,id
+wall,0,102,51,255,0
+building,0,255,0,255,1
+sky,4,74,246,255,2
+floor,191,191,181,255,3
+tree,72,90,13,255,4
+ceiling,204,0,102,255,5
+road,196,26,252,255,6
+bed,254,217,251,255,7
+windowpane,127,255,255,255,8
+grass,133,87,54,255,9
+cabinet,130,26,50,255,10
+sidewalk,10,5,65,255,11
+person,3,75,135,255,12
+earth,18,124,89,255,13
+door,21,182,126,255,14
+table,90,171,130,255,15
+mountain,189,217,119,255,16
+plant,188,63,11,255,17
+curtain,86,82,249,255,18
+chair,196,221,253,255,19
+car,187,78,62,255,20
+water,188,49,167,255,21
+painting,182,161,248,255,22
+sofa,112,201,196,255,23
+shelf,11,41,99,255,24
+house,211,102,1,255,25
+sea,4,227,101,255,26
+mirror,147,201,250,255,27
+rug,255,0,0,255,28
+field,224,33,3,255,29
+armchair,254,74,17,255,30
+seat,255,127,0,255,31
+fence,176,188,66,255,32
+desk,247,29,54,255,33
+rock,74,117,80,255,34
+wardrobe,71,56,199,255,35
+lamp,250,107,216,255,36
+bathtub,4,204,229,255,37
+railing,25,208,53,255,38
+cushion,52,249,65,255,39
+base,70,220,121,255,40
+box,68,86,156,255,41
+column,7,150,36,255,42
+signboard,255,127,127,255,43
+chest,64,46,6,255,44
+counter,139,167,123,255,45
+sand,18,165,78,255,46
+sink,157,166,28,255,47
+skyscraper,9,163,248,255,48
+fireplace,191,67,218,255,49
+refrigerator,135,243,61,255,50
+grandstand,81,119,190,255,51
+path,0,126,204,255,52
+stairs,228,73,252,255,53
+runway,227,108,163,255,54
+case,127,255,127,255,55
+pool,50,211,203,255,56
+pillow,3,190,177,255,57
+screen,127,255,0,255,58
+stairway,11,59,10,255,59
+river,108,38,143,255,60
+bridge,243,33,254,255,61
+bookcase,182,106,251,255,62
+blind,120,251,191,255,63
+coffee,145,115,208,255,64
+toilet,48,129,250,255,65
+flower,43,78,65,255,66
+book,73,0,197,255,67
+hill,0,249,143,255,68
+bench,80,160,69,255,69
+countertop,200,252,62,255,70
+stove,127,39,198,255,71
+palm,139,79,186,255,72
+kitchen,155,225,169,255,73
+computer,239,148,63,255,74
+swivel,30,183,5,255,75
+boat,137,90,3,255,76
+bar,49,35,155,255,77
+arcade,242,186,113,255,78
+hovel,127,0,0,255,79
+bus,191,148,1,255,80
+towel,134,147,65,255,81
+light,175,254,104,255,82
+truck,84,183,23,255,83
+tower,12,87,183,255,84
+chandelier,1,20,178,255,85
+awning,185,224,0,255,86
+streetlight,255,255,127,255,87
+booth,44,252,176,255,88
+television,62,186,252,255,89
+airplane,3,253,46,255,90
+dirt,41,208,156,255,91
+apparel,138,94,126,255,92
+pole,32,31,231,255,93
+land,192,120,193,255,94
+bannister,231,66,87,255,95
+escalator,105,0,153,255,96
+ottoman,251,216,190,255,97
+bottle,228,45,198,255,98
+buffet,0,0,255,255,99
+poster,67,234,247,255,100
+stage,156,4,152,255,101
+van,253,219,57,255,102
+ship,183,148,126,255,103
+fountain,246,154,179,255,104
+conveyer,127,127,255,255,105
+canopy,0,255,255,255,106
+washer,225,189,224,255,107
+plaything,47,253,125,255,108
+swimming,47,0,127,255,109
+stool,92,207,58,255,110
+barrel,173,253,235,255,111
+basket,251,78,170,255,112
+waterfall,164,41,91,255,113
+tent,0,127,0,255,114
+bag,65,43,54,255,115
+minibike,196,100,129,255,116
+cradle,143,157,184,255,117
+oven,234,190,2,255,118
+ball,0,234,191,255,119
+food,191,254,179,255,120
+step,149,1,215,255,121
+tank,67,231,11,255,122
+trade,131,195,0,255,123
+microwave,188,121,62,255,124
+pot,70,143,13,255,125
+animal,106,168,230,255,126
+bicycle,231,106,88,255,127
+lake,255,255,0,255,128
+dishwasher,121,54,15,255,129
+screen,127,255,0,255,130
+blanket,248,157,252,255,131
+sculpture,63,7,14,255,132
+hood,146,63,249,255,133
+sconce,185,8,32,255,134
+vase,247,0,223,255,135
+traffic,234,252,199,255,136
+tray,91,29,248,255,137
+ashcan,122,0,89,255,138
+fan,41,99,220,255,139
+pier,28,132,140,255,140
+crt,127,209,111,255,141
+plate,110,54,79,255,142
+monitor,63,68,107,255,143
+bulletin,214,0,171,255,144
+shower,71,9,82,255,145
+radiator,224,187,57,255,146
+glass,253,20,130,255,147
+clock,127,127,0,255,148
+flag,113,132,132,255,149

hydra_ros/config/datasets/bosch_stretch.yaml

@@ -0,0 +1,14 @@
+---
+type: RosInput
+tf_verbosity: 1
+clear_queue_on_fail: true
+receivers:
+  - type: ImageReceiver
+    sensor:
+      type: camera_info
+      min_range: $(arg sensor_min_range)
+      max_range: $(arg sensor_max_range)
+      camera_info_topic: $(arg rgb_info_topic)
+      extrinsics:
+        type: ros
+        sensor_frame: $(arg sensor_frame)

hydra_ros/launch/datasets/bosch_stretch.launch

@@ -0,0 +1,68 @@
+<launch>
+    <arg name="sim_time_required" default="true"/>
+    <param name="use_sim_time" value="$(arg sim_time_required)"/>
+
+    <arg name="use_prerecorded_semantics" default="false"/>
+
+    <arg name="config_dir" default="$(find hydra)/config/uhumans2"/>
+    <arg name="use_gt_frame" default="true" doc="use simulator-provided tf frame"/>
+    <arg name="use_gt_semantics" default="false" doc="use simulator-provider semantics"/>
+    <arg name="use_static_tfs" default="true" doc="publish static tfs from file"/>
+    <arg name="color_mesh_by_label" default="false" doc="display mesh colored by semantic label"/>
+    <arg name="use_oriented_bounding_boxes" default="false"/>
+    <arg name="use_2d_places" default="true"/>
+    <arg name="use_single_channel_label_image" default="false"/>
+
+    <!-- TODO(blake.buchanan): Verify robot_frame, odom_frame, and sensor_frame from stretch data. -->
+    <arg name="robot_frame" default="$(eval 'base_link' if arg('use_gt_frame') else 'base_link_kimera')" doc="robot body tf frame"/>
+    <arg name="odom_frame" default="$(eval 'world' if arg('use_gt_frame') else 'odom')" doc="odometry (map) frame"/>
+    <arg name="sensor_frame" default="$(eval 'camera_color_optical_frame' if arg('use_gt_frame') else 'left_cam_kimera')" doc="camera frame"/>
+
+    <!-- semantics -->
+    <arg name="labelspace_name" default="$(eval 'bosch_office' if arg('use_gt_semantics') else 'bosch_office')" doc="semantic label space"/>
+    <arg name="semantic_map_dir" default="$(find hydra_ros)/config/color" if="$(arg use_gt_semantics)"/>
+    <arg name="semantic_map_dir" default="$(find semantic_inference)/config/colors" unless="$(arg use_gt_semantics)"/>
+    <arg name="semantic_map_path" default="$(arg semantic_map_dir)/$(arg labelspace_name).csv"/>
+    <arg name="semantic_label_remap_filepath" default="$(find hydra)/config/label_remaps/bosch_office.yaml" if="$(arg use_single_channel_label_image)"/>
+
+    <arg name="rgb_topic" default="/camera/color/image_raw"/> <!--/tesse/left_cam/rgb/image_raw -->
+    <arg name="rgb_info_topic" default="/camera/color/camera_info"/> <!-- /tesse/left_cam/camera_info -->
+    <arg name="depth_topic" default="/camera/depth/image_raw_corrected"/> <!-- /tesse/depth_cam/mono/image_raw -->
+    <arg name="label_topic" default="semanic_inference/semantic/image_raw" unless="$(arg use_gt_semantics)"/>
+
+    <!-- Since we don't have segmentation images, we use the vlm_planner_eqa_ros node to taken in segmented FeatureImages, convert them and then send them over /camera/semantic/image_raw -->
+    <arg name="label_topic" default="$(eval '/camera/semantic/image_raw' if arg('use_single_channel_label_image') else '/camera/semantic/image_raw')"
+                            if="$(arg use_gt_semantics)"/>
+
+    <arg name="prerecorded_topic" default="/oneformer/labels/image_raw"/>
+
+    <!-- This static TF does not influence the generation of the scene graph... -->
+    <include file="$(find hydra_ros)/launch/static_tfs/bosch_stretch_static_tfs.xml" if="$(arg use_static_tfs)"/>
+
+    <!-- <node pkg="tf2_ros" type="static_transform_publisher" name="fake_world_tf" args="0 0 0 0 0 0 1 map odom" if="$(arg use_gt_frame)"/> -->
+
+    <group unless="$(arg use_gt_semantics)">
+        <remap from="semantic_inference/color/image_raw" to="$(arg rgb_topic)"/>
+        <remap from="semantic_inference/labels/image_raw" to="$(arg prerecorded_topic)" if="$(arg use_prerecorded_semantics)"/>
+        <include file="$(find semantic_inference_ros)/launch/semantic_inference.launch" pass_all_args="true"/>
+    </group>
+
+    <node pkg="vlm_planner_semnav"
+          type="vlm_planner_eqa_ros.py"
+          name="vlm_planner_eqa_ros"
+          output="screen"/>
+
+    <arg name="start_hydra" default="true"/>
+    <group if="$(arg start_hydra)">
+        <include file="$(find hydra_ros)/launch/hydra.launch" pass_all_args="true">
+            <arg name="dataset_name" default="bosch_stretch"/>
+            <arg name="rviz_file" default="bosch_stretch.rviz"/>
+        </include>
+    </group>
+
+    <!-- <include file="$(find hydra_ros)/launch/hydra.launch" pass_all_args="true">
+        <arg name="dataset_name" default="bosch_stretch"/>
+        <arg name="rviz_file" default="bosch_stretch.rviz"/>
+    </include> -->
+
+</launch>

hydra_ros/launch/static_tfs/bosch_stretch_static_tfs.xml

@@ -0,0 +1,15 @@
+<launch>
+
+    <!-- <node pkg="tf2_ros" type="static_transform_publisher" name="bag_static_tf_0" args="0.0 0.05 0.0 0.5 -0.5 0.5 -0.5 base_link_gt left_cam"/>
+
+    <node pkg="tf2_ros" type="static_transform_publisher" name="bag_static_tf_1" args="0.0 -0.05 0.0 0.5 -0.5 0.5 -0.5 base_link_gt right_cam"/>
+
+    <node pkg="tf2_ros" type="static_transform_publisher" name="bag_static_tf_2" args="0.0 0.05 0.0 0.5 -0.5 0.5 -0.5 base_link_gt left_cam"/>
+
+    <node pkg="tf2_ros" type="static_transform_publisher" name="bag_static_tf_3" args="0.0 0.05 0.0 0.5 -0.5 0.5 -0.5 base_link_gt left_cam"/>
+
+    <node pkg="tf2_ros" type="static_transform_publisher" name="bag_static_tf_4" args="0.17 -0.017 0.0 0.0 0.0 0.0 1.0 base_link_gt front_lidar"/>
+
+    <node pkg="tf2_ros" type="static_transform_publisher" name="bag_static_tf_5" args="-0.17 -0.017 0.0 0.0 0.0 0.0 1.0 base_link_gt rear_lidar"/> -->
+
+</launch>