diff --git a/hydra_ros/config/color/bosch_office.csv b/hydra_ros/config/color/bosch_office.csv
new file mode 100644
index 0000000..aaeb0a0
--- /dev/null
+++ b/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
diff --git a/hydra_ros/config/datasets/bosch_stretch.yaml b/hydra_ros/config/datasets/bosch_stretch.yaml
new file mode 100644
index 0000000..c580d98
--- /dev/null
+++ b/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)
diff --git a/hydra_ros/launch/datasets/bosch_stretch.launch b/hydra_ros/launch/datasets/bosch_stretch.launch
new file mode 100644
index 0000000..18b70d6
--- /dev/null
+++ b/hydra_ros/launch/datasets/bosch_stretch.launch
@@ -0,0 +1,68 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/hydra_ros/launch/static_tfs/bosch_stretch_static_tfs.xml b/hydra_ros/launch/static_tfs/bosch_stretch_static_tfs.xml
new file mode 100644
index 0000000..318a04f
--- /dev/null
+++ b/hydra_ros/launch/static_tfs/bosch_stretch_static_tfs.xml
@@ -0,0 +1,15 @@
+
+
+
+
+
diff --git a/hydra_ros/rviz/bosch_stretch.rviz b/hydra_ros/rviz/bosch_stretch.rviz
new file mode 100644
index 0000000..7172f55
--- /dev/null
+++ b/hydra_ros/rviz/bosch_stretch.rviz
@@ -0,0 +1,476 @@
+Panels:
+ - Class: rviz/Displays
+ Help Height: 0
+ Name: Displays
+ Property Tree Widget:
+ Expanded:
+ - /Global Options1
+ - /TF1
+ - /Scene Graph1
+ Splitter Ratio: 0.5
+ Tree Height: 777
+ - Class: rviz/Selection
+ Name: Selection
+ - Class: rviz/Tool Properties
+ Expanded:
+ - /2D Pose Estimate1
+ - /2D Nav Goal1
+ - /Publish Point1
+ Name: Tool Properties
+ Splitter Ratio: 0.5886790156364441
+ - Class: rviz/Views
+ Expanded:
+ - /Current View1
+ Name: Views
+ Splitter Ratio: 0.5
+ - Class: rviz/Time
+ Name: Time
+ SyncMode: 0
+ SyncSource: Image
+Preferences:
+ PromptSaveOnExit: true
+Toolbars:
+ toolButtonStyle: 2
+Visualization Manager:
+ Class: ""
+ Displays:
+ - Class: rviz/TF
+ Enabled: true
+ Filter (blacklist): ""
+ Filter (whitelist): ""
+ Frame Timeout: 15
+ Frames:
+ All Enabled: true
+ base_imu:
+ Value: true
+ base_link:
+ Value: true
+ camera_accel_frame:
+ Value: true
+ camera_accel_optical_frame:
+ Value: true
+ camera_aligned_depth_to_color_frame:
+ Value: true
+ camera_bottom_screw_frame:
+ Value: true
+ camera_color_frame:
+ Value: true
+ camera_color_optical_frame:
+ Value: true
+ camera_depth_frame:
+ Value: true
+ camera_depth_optical_frame:
+ Value: true
+ camera_gyro_frame:
+ Value: true
+ camera_gyro_optical_frame:
+ Value: true
+ camera_infra1_frame:
+ Value: true
+ camera_infra1_optical_frame:
+ Value: true
+ camera_infra2_frame:
+ Value: true
+ camera_infra2_optical_frame:
+ Value: true
+ camera_link:
+ Value: true
+ caster_link:
+ Value: true
+ laser:
+ Value: true
+ link_arm_l0:
+ Value: true
+ link_arm_l1:
+ Value: true
+ link_arm_l2:
+ Value: true
+ link_arm_l3:
+ Value: true
+ link_arm_l4:
+ Value: true
+ link_aruco_inner_wrist:
+ Value: true
+ link_aruco_left_base:
+ Value: true
+ link_aruco_right_base:
+ Value: true
+ link_aruco_shoulder:
+ Value: true
+ link_aruco_top_wrist:
+ Value: true
+ link_grasp_center:
+ Value: true
+ link_gripper:
+ Value: true
+ link_gripper_finger_left:
+ Value: true
+ link_gripper_finger_right:
+ Value: true
+ link_gripper_fingertip_left:
+ Value: true
+ link_gripper_fingertip_right:
+ Value: true
+ link_head:
+ Value: true
+ link_head_pan:
+ Value: true
+ link_head_tilt:
+ Value: true
+ link_left_wheel:
+ Value: true
+ link_lift:
+ Value: true
+ link_mast:
+ Value: true
+ link_right_wheel:
+ Value: true
+ link_wrist_yaw:
+ Value: true
+ map:
+ Value: true
+ odom:
+ Value: true
+ respeaker_base:
+ Value: true
+ scanmatcher_frame:
+ Value: true
+ Marker Alpha: 1
+ Marker Scale: 1
+ Name: TF
+ Show Arrows: true
+ Show Axes: true
+ Show Names: true
+ Tree:
+ map:
+ odom:
+ base_link:
+ base_imu:
+ {}
+ caster_link:
+ {}
+ laser:
+ {}
+ link_aruco_left_base:
+ {}
+ link_aruco_right_base:
+ {}
+ link_left_wheel:
+ {}
+ link_mast:
+ link_head:
+ link_head_pan:
+ link_head_tilt:
+ camera_bottom_screw_frame:
+ camera_link:
+ camera_accel_frame:
+ camera_accel_optical_frame:
+ {}
+ camera_aligned_depth_to_color_frame:
+ camera_color_optical_frame:
+ {}
+ camera_color_frame:
+ {}
+ camera_depth_frame:
+ camera_depth_optical_frame:
+ {}
+ camera_gyro_frame:
+ camera_gyro_optical_frame:
+ {}
+ camera_infra1_frame:
+ camera_infra1_optical_frame:
+ {}
+ camera_infra2_frame:
+ camera_infra2_optical_frame:
+ {}
+ link_lift:
+ link_arm_l4:
+ link_arm_l3:
+ link_arm_l2:
+ link_arm_l1:
+ link_arm_l0:
+ link_aruco_inner_wrist:
+ {}
+ link_aruco_top_wrist:
+ {}
+ link_wrist_yaw:
+ link_gripper:
+ link_grasp_center:
+ {}
+ link_gripper_finger_left:
+ link_gripper_fingertip_left:
+ {}
+ link_gripper_finger_right:
+ link_gripper_fingertip_right:
+ {}
+ link_aruco_shoulder:
+ {}
+ respeaker_base:
+ {}
+ link_right_wheel:
+ {}
+ scanmatcher_frame:
+ {}
+ Update Interval: 0
+ Value: true
+ - Class: rviz/Group
+ Displays:
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /hydra_dsg_visualizer/dsg_markers
+ Name: Scene Graph (Static)
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: true
+ - Class: rviz/MarkerArray
+ Enabled: true
+ Marker Topic: /hydra_dsg_visualizer/dynamic_layers_viz
+ Name: Scene Graph (Dynamic)
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: true
+ - Class: kimera_pgmo_rviz/MeshDisplay
+ Cull Backfaces: true
+ Enable Lighting: false
+ Enabled: true
+ Name: Mesh
+ Queue Size: 10
+ Toggle Visibility All: true
+ Topic: /hydra_dsg_visualizer/dsg_mesh
+ Unreliable: false
+ Value: true
+ Visible: true
+ Enabled: true
+ Name: Scene Graph
+ - Class: rviz/Group
+ Displays:
+ - Alpha: 1
+ Autocompute Intensity Bounds: true
+ Autocompute Value Bounds:
+ Max Value: 10
+ Min Value: -10
+ Value: true
+ Axis: Z
+ Channel Name: intensity
+ Class: rviz/PointCloud2
+ Color: 255; 255; 255
+ Color Transformer: RGB8
+ Decay Time: 0
+ Enabled: false
+ Invert Rainbow: false
+ Max Color: 255; 255; 255
+ Min Color: 0; 0; 0
+ Name: Input Pointcloud
+ Position Transformer: XYZ
+ Queue Size: 10
+ Selectable: true
+ Size (Pixels): 3
+ Size (m): 0.009999999776482582
+ Style: Flat Squares
+ Topic: /semantic_pointcloud
+ Unreliable: false
+ Use Fixed Frame: true
+ Use rainbow: true
+ Value: false
+ - Class: rviz/Image
+ Enabled: false
+ Image Topic: /camera/aligned_depth_to_color/image_raw
+ Max Value: 1
+ Median window: 300
+ Min Value: 0
+ Name: Depth
+ Normalize Range: true
+ Queue Size: 2
+ Transport Hint: raw
+ Unreliable: false
+ Value: false
+ - Class: rviz/Image
+ Enabled: false
+ Image Topic: /tesse/seg_cam/rgb/image_raw
+ Max Value: 1
+ Median window: 5
+ Min Value: 0
+ Name: Semantics
+ Normalize Range: true
+ Queue Size: 2
+ Transport Hint: raw
+ Unreliable: false
+ Value: false
+ - Class: rviz/Image
+ Enabled: false
+ Image Topic: /camera/color/image_raw
+ Max Value: 1
+ Median window: 5
+ Min Value: 0
+ Name: RGB
+ Normalize Range: true
+ Queue Size: 2
+ Transport Hint: raw
+ Unreliable: false
+ Value: false
+ Enabled: true
+ Name: Input
+ - Class: rviz/Group
+ Displays:
+ - Class: rviz/Marker
+ Enabled: true
+ Marker Topic: /incremental_dsg_builder_node/pgmo/deformation_graph_mesh_mesh
+ Name: Mesh Edges
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: true
+ - Class: rviz/Marker
+ Enabled: true
+ Marker Topic: /incremental_dsg_builder_node/pgmo/deformation_graph_pose_mesh
+ Name: Pose-Mesh Edges
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: true
+ Enabled: false
+ Name: Factor Graph
+ - Class: rviz/Group
+ Displays:
+ - Class: rviz/MarkerArray
+ Enabled: false
+ Marker Topic: /hydra_ros_node/places/gvd_graph_viz
+ Name: GVD Graph
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: false
+ - Class: rviz/MarkerArray
+ Enabled: false
+ Marker Topic: /hydra_ros_node/places/graph_viz
+ Name: Sparse GVD Graph
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: false
+ - Class: rviz/MarkerArray
+ Enabled: false
+ Marker Topic: /hydra_ros_node/places/gvd_cluster_viz
+ Name: GVD Clusters
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: false
+ - Class: rviz/MarkerArray
+ Enabled: false
+ Marker Topic: /hydra_ros_node/places/esdf_viz
+ Name: EsdfSlice
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: false
+ Enabled: true
+ Name: Places
+ - Class: rviz/Group
+ Displays:
+ - Class: rviz/MarkerArray
+ Enabled: false
+ Marker Topic: /hydra_ros_node/reconstruction/tsdf_viz
+ Name: TSDF Distances
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: false
+ - Class: rviz/MarkerArray
+ Enabled: false
+ Marker Topic: /hydra_ros_node/reconstruction/tsdf_weight_viz
+ Name: TSDF Weights
+ Namespaces:
+ {}
+ Queue Size: 100
+ Value: false
+ Enabled: true
+ Name: Reconstruction
+ - Class: rviz/Image
+ Enabled: true
+ Image Topic: /camera/color/image_raw
+ Max Value: 1
+ Median window: 5
+ Min Value: 0
+ Name: Image
+ Normalize Range: true
+ Queue Size: 2
+ Transport Hint: raw
+ Unreliable: false
+ Value: true
+ Enabled: true
+ Global Options:
+ Background Color: 255; 255; 255
+ Default Light: true
+ Fixed Frame: map
+ Frame Rate: 30
+ Name: root
+ Tools:
+ - Class: rviz/Interact
+ Hide Inactive Objects: true
+ - Class: rviz/MoveCamera
+ - Class: rviz/Select
+ - Class: rviz/FocusCamera
+ - Class: rviz/Measure
+ - Class: rviz/SetInitialPose
+ Theta std deviation: 0.2617993950843811
+ Topic: /initialpose
+ X std deviation: 0.5
+ Y std deviation: 0.5
+ - Class: rviz/SetGoal
+ Topic: /move_base_simple/goal
+ - Class: rviz/PublishPoint
+ Single click: true
+ Topic: /clicked_point
+ Value: true
+ Views:
+ Current:
+ Class: rviz/Orbit
+ Distance: 32.54032516479492
+ Enable Stereo Rendering:
+ Stereo Eye Separation: 0.05999999865889549
+ Stereo Focal Distance: 1
+ Swap Stereo Eyes: false
+ Value: false
+ Field of View: 0.7853981852531433
+ Focal Point:
+ X: -9.634339332580566
+ Y: 23.9942626953125
+ Z: 10.865774154663086
+ Focal Shape Fixed Size: false
+ Focal Shape Size: 0.05000000074505806
+ Invert Z Axis: false
+ Name: Current View
+ Near Clip Distance: 0.009999999776482582
+ Pitch: 0.7147967219352722
+ Target Frame:
+ Yaw: 3.111708164215088
+ Saved: ~
+Window Geometry:
+ Depth:
+ collapsed: false
+ Displays:
+ collapsed: false
+ Height: 1136
+ Hide Left Dock: false
+ Hide Right Dock: true
+ Image:
+ collapsed: false
+ QMainWindow State: 000000ff00000000fd0000000400000000000001560000041afc020000000cfb0000001200530065006c0065006300740069006f006e000000003b0000018f0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000005fb000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000344000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a00440065007000740068000000036d0000006e0000001600fffffffb0000001200530065006d0061006e00740069006300730000000382000000590000001600fffffffb00000006005200470042000000020a000001d10000001600fffffffb0000000a0049006d0061006700650100000385000000d00000001600ffffff000000010000010f00000351fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b00000351000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650200000422000002580000035e000001e0fb0000000800540069006d00650100000000000004500000000000000000000006240000041a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+ RGB:
+ collapsed: false
+ Selection:
+ collapsed: false
+ Semantics:
+ collapsed: false
+ Time:
+ collapsed: false
+ Tool Properties:
+ collapsed: false
+ Views:
+ collapsed: true
+ Width: 1920
+ X: 0
+ Y: 27