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