Feature/simmon updates (#142)

* update launch files to new convention and set fixed attempt number for tf lookup

* fix issue with can transform hanging

* separate extrinsics and topology config

* rename and update hydra launch files

* fix uh2 extrinsics

* add sparkal extrinsics

Co-authored-by: Nathan Hughes <[email protected]>

README.md

@@ -112,7 +112,7 @@ Make sure to decompress the rosbag (`rosbag decompress path/to/bagfile`) before
 
 To start Hydra:
 ```
-roslaunch hydra_dsg_builder uhumans2_incremental_dsg.launch start_visualizer:=true
+roslaunch hydra_dsg_builder_ros uhumans2.launch start_visualizer:=true
 ```
 
 Then, start the rosbag in a separate terminal:
@@ -143,7 +143,8 @@ roslaunch kimera_vio_ros kimera_vio_ros kimera_vio_ros_uhumans2.launch online:=t
 and in a separate terminal, run:
 
 ```
-roslaunch hydra_dsg_builder uhumans2_incremental_dsg.launch \
+# in separate terminal
+roslaunch hydra_dsg_builder_ros uhumans2.launch \
      start_visualizer:=true \
      use_gt_frame:=false
 ```
@@ -161,7 +162,7 @@ roslaunch kimera_vio_ros kimera_vio_ros kimera_vio_ros_uhumans2.launch online:=t
 and in a separate terminal, run the same command for Hydra:
 
 ```
-roslaunch hydra_dsg_builder uhumans2_incremental_dsg.launch \
+roslaunch hydra_dsg_builder uhumans2.launch \
      start_visualizer:=true \
      use_gt_frame:=false
 ```
@@ -181,7 +182,7 @@ roslaunch kimera_vio_ros kimera_vio_ros kimera_vio_ros_uhumans2.launch online:=t
 and in a separate terminal, run the same command for Hydra:
 
 ```
-roslaunch hydra_dsg_builder uhumans2_incremental_dsg.launch \
+roslaunch hydra_dsg_builder_ros uhumans2.launch \
      start_visualizer:=true \
      use_gt_frame:=false \
      enable_dsg_lcd:=true

hydra_dsg_builder_ros/launch/dsg_builder.launch → hydra_dsg_builder_ros/launch/hydra.launch

@@ -32,6 +32,7 @@
 
     <arg name="do_reconstruction" default="true"/>
     <arg name="reconstruction_config" if="$(arg do_reconstruction)"/>
+    <arg name="extrinsics" if="$(arg do_reconstruction)"/>
     <arg name="pointcloud_topic" if="$(arg do_reconstruction)"/>
     <arg name="use_ros_backend" default="false"/>
 
@@ -67,6 +68,7 @@
         <rosparam file="$(arg config_dir)/dsg_backend_config.yaml"/>
         <rosparam file="$(arg config_dir)/dsg_lcd_config.yaml"/>
         <rosparam file="$(arg reconstruction_config)" if="$(arg do_reconstruction)"/>
+        <rosparam file="$(arg extrinsics)" if="$(arg do_reconstruction)"/>
 
         <param name="use_pose_graph" value="$(eval not arg('use_gt_frame'))"/>
         <param name="do_reconstruction" value="$(arg do_reconstruction)"/>

hydra_dsg_builder_ros/launch/mp3d_incremental_dsg.launch → hydra_dsg_builder_ros/launch/mp3d.launch

@@ -29,6 +29,6 @@
         <arg name="debug" value="false"/>
     </include>
 
-    <include file="$(find hydra_dsg_builder_ros)/launch/dsg_builder.launch" pass_all_args="true"/>
+    <include file="$(find hydra_dsg_builder_ros)/launch/hydra.launch" pass_all_args="true"/>
 
 </launch>

hydra_dsg_builder_ros/launch/sidpac_incremental_dsg.launch → hydra_dsg_builder_ros/launch/sidpac.launch

@@ -13,12 +13,13 @@
 
     <include file="$(find hydra_utils)/launch/data_sources/sidpac_pointcloud.launch" pass_all_args="true"/>
 
-    <include file="$(find hydra_dsg_builder_ros)/launch/dsg_builder.launch" pass_all_args="true">
+    <include file="$(find hydra_dsg_builder_ros)/launch/hydra.launch" pass_all_args="true">
         <arg name="robot_id" default="0"/>
         <arg name="config_dir" default="$(find hydra_dsg_builder)/config/sidpac"/>
         <arg name="dsg_output_dir" default="$(find hydra_dsg_builder)/output/sidpac"/>
         <arg name="dsg_output_prefix" default="floor1_3"/>
         <arg name="reconstruction_config" default="$(find hydra_topology)/config/sidpac_topology_config.yaml"/>
+        <arg name="extrinsics" default="$(find hydra_topology)/config/extrinsics/sidpac.yaml"/>
         <arg name="rviz_dir" default="$(find hydra_dsg_builder_ros)/rviz"/>
         <arg name="rviz_file" default="sidpac.rviz"/>
         <arg name="use_oriented_bounding_boxes" default="true"/>

hydra_dsg_builder_ros/launch/simmons_jackal.launch

@@ -0,0 +1,36 @@
+<launch>
+    <arg name="sim_time_required" default="true"/>
+    <param name="use_sim_time" value="$(arg sim_time_required)"/>
+
+    <!-- TODO(nathan) fix this arg name -->
+    <arg name="dataset_name" default="ade150_config"/>
+    <arg name="semantic_map_dir" default="$(find semantic_recolor)/config/colors"/>
+    <arg name="semantic_map_file" default="$(arg dataset_name).csv"/>
+    <arg name="semantic_map_path" default="$(arg semantic_map_dir)/$(arg semantic_map_file)"/>
+
+    <arg name="robot_name" default="sparkal1"/>
+
+    <arg name="pointcloud_topic" value="/$(arg robot_name)/semantic_pointcloud"/>
+    <arg name="use_gt_frame" default="true"/>
+    <arg name="sensor_frame" default="$(arg robot_name)/forward_imu_optical_frame" if="$(arg use_gt_frame)"/>
+    <arg name="sensor_frame" default="base_link" unless="$(arg use_gt_frame)"/>
+    <arg name="world_frame" default="$(arg robot_name)/map"/>
+
+    <include file="$(find hydra_utils)/launch/data_sources/simmons_jackal_pointcloud.launch"
+             pass_all_args="true"/>
+
+    <include file="$(find hydra_dsg_builder_ros)/launch/hydra.launch" pass_all_args="true">
+        <arg name="robot_id" default="0"/>
+        <arg name="config_dir" default="$(find hydra_dsg_builder)/config/simmons_jackal"/>
+        <arg name="dsg_output_dir" default="$(find hydra_dsg_builder)/output/simmons_jackal"/>
+        <arg name="dsg_output_prefix" default="simmons_6"/>
+        <arg name="reconstruction_config" default="$(find hydra_topology)/config/simmons_topology_config.yaml"/>
+        <arg name="extrinsics" default="$(find hydra_topology)/config/extrinsics/$(arg robot_name).yaml"/>
+        <arg name="rviz_dir" default="$(find hydra_dsg_builder_ros)/rviz"/>
+        <arg name="rviz_file" default="simmons.rviz"/>
+        <arg name="use_oriented_bounding_boxes" default="true"/>
+        <arg name="typology_dir" default="$(find hydra_dsg_builder)/config/simmons_jackal"/>
+        <arg name="typology_config" default="simmons_jackal_typology.yaml"/>
+    </include>
+
+</launch>

hydra_dsg_builder_ros/launch/uhumans2_incremental_dsg.launch → hydra_dsg_builder_ros/launch/uhumans2.launch

@@ -20,12 +20,13 @@
 
     <include file="$(find hydra_utils)/launch/data_sources/uhumans2_pointcloud.launch" pass_all_args="true"/>
 
-    <include file="$(find hydra_dsg_builder_ros)/launch/dsg_builder.launch" pass_all_args="true">
+    <include file="$(find hydra_dsg_builder_ros)/launch/hydra.launch" pass_all_args="true">
         <arg name="robot_id" default="0"/>
         <arg name="config_dir" default="$(find hydra_dsg_builder)/config/uhumans2"/>
         <arg name="dsg_output_dir" default="$(find hydra_dsg_builder)/output/uhumans2"/>
         <arg name="dsg_output_prefix" default="office"/>
         <arg name="reconstruction_config" default="$(find hydra_topology)/config/uhumans2_topology_config.yaml"/>
+        <arg name="extrinsics" default="$(find hydra_topology)/config/extrinsics/uhumans2.yaml"/>
         <arg name="rviz_dir" default="$(find hydra_dsg_builder_ros)/rviz"/>
         <arg name="rviz_file" default="uhumans2.rviz"/>
         <arg name="use_oriented_bounding_boxes" default="false"/>