Add optical frame publisher (#413)

* check topic heartbeat and log /X*/pose topics * use pose_v and subt to pose_static * cleanup * Rename parameters * move heartbeat check into its own thread * add optical frame publisher and update vehicle topics Signed-off-by: Ian Chen <[email protected]> * fix rotation Signed-off-by: Ian Chen <[email protected]> * update optical frame publisher args Signed-off-by: Ian Chen <[email protected]> * lazy subscription Signed-off-by: Ian Chen <[email protected]> Co-authored-by: Nate Koenig <[email protected]>
osrf · May 14, 2020 · 0eed307 · 0eed307
1 parent e615b31
commit 0eed307
Show file tree

Hide file tree

Showing 15 changed files with 688 additions and 3 deletions.
diff --git a/submitted_models/costar_husky_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/costar_husky_sensor_config_1/launch/vehicle_topics.launch
@@ -65,6 +65,17 @@
       args="$(arg sensor_prefix)/camera_front/image">
       <remap from="$(arg sensor_prefix)/camera_front/image" to="front/image_raw"/>
     </node>
+
+    <node
+      pkg="subt_ros"
+      type="optical_frame_publisher"
+      name="optical_frame_publisher">
+      <remap from="input/image" to="front/image_raw" />
+      <remap from="output/image" to="front/optical/image_raw" />
+      <remap from="input/camera_info" to="front/camera_info" />
+      <remap from="output/camera_info" to="front/optical/camera_info" />
+    </node>
+
     <node
       pkg="ros_ign_bridge"
       type="parameter_bridge"
@@ -80,6 +91,15 @@
       <remap from="$(arg sensor_prefix)/camera_front/depth_image" to="front/depth"/>
     </node>
 
+    <node
+      pkg="subt_ros"
+      type="optical_frame_publisher"
+      name="optical_frame_publisher_depth"
+      args="depth">
+      <remap from="input/image" to="front/depth" />
+      <remap from="output/image" to="front/optical/depth" />
+    </node>
+
     <!--Front Laser-->
     <node
       pkg="ros_ign_bridge"

diff --git a/submitted_models/explorer_x1_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/explorer_x1_sensor_config_1/launch/vehicle_topics.launch
@@ -65,6 +65,17 @@
         args="$(arg sensor_prefix)/front_realsense/image">
         <remap from="$(arg sensor_prefix)/front_realsense/image" to="image_raw"/>
       </node>
+
+      <node
+        pkg="subt_ros"
+        type="optical_frame_publisher"
+        name="optical_frame_publisher">
+        <remap from="input/image" to="image_raw" />
+        <remap from="output/image" to="optical/image_raw" />
+        <remap from="input/camera_info" to="camera_info" />
+        <remap from="output/camera_info" to="optical/camera_info" />
+      </node>
+
       <node
         pkg="ros_ign_bridge"
         type="parameter_bridge"
@@ -79,6 +90,16 @@
         args="$(arg sensor_prefix)/front_realsense/depth_image">
         <remap from="$(arg sensor_prefix)/front_realsense/depth_image" to="depth/image_raw"/>
       </node>
+
+      <node
+        pkg="subt_ros"
+        type="optical_frame_publisher"
+        name="optical_frame_publisher_depth"
+        args="depth">
+        <remap from="input/image" to="depth/image_raw" />
+        <remap from="output/image" to="depth/optical/image_raw" />
+      </node>
+
     </group>
 
     <!--Left RGBD camera -->
@@ -97,6 +118,17 @@
         args="$(arg sensor_prefix)/left_realsense/image">
         <remap from="$(arg sensor_prefix)/left_realsense/image" to="image_raw"/>
       </node>
+
+      <node
+        pkg="subt_ros"
+        type="optical_frame_publisher"
+        name="optical_frame_publisher">
+        <remap from="input/image" to="image_raw" />
+        <remap from="output/image" to="optical/image_raw" />
+        <remap from="input/camera_info" to="camera_info" />
+        <remap from="output/camera_info" to="optical/camera_info" />
+      </node>
+
       <node
         pkg="ros_ign_bridge"
         type="parameter_bridge"
@@ -111,6 +143,16 @@
         args="$(arg sensor_prefix)/left_realsense/depth_image">
         <remap from="$(arg sensor_prefix)/left_realsense/depth_image" to="depth/image_raw"/>
       </node>
+
+      <node
+        pkg="subt_ros"
+        type="optical_frame_publisher"
+        name="optical_frame_publisher_depth"
+        args="depth">
+        <remap from="input/image" to="depth/image_raw" />
+        <remap from="output/image" to="depth/optical/image_raw" />
+      </node>
+
     </group>
 
     <!--Rear RGBD camera -->
@@ -129,6 +171,17 @@
         args="$(arg sensor_prefix)/rear_realsense/image">
         <remap from="$(arg sensor_prefix)/rear_realsense/image" to="image_raw"/>
       </node>
+
+      <node
+        pkg="subt_ros"
+        type="optical_frame_publisher"
+        name="optical_frame_publisher">
+        <remap from="input/image" to="image_raw" />
+        <remap from="output/image" to="optical/image_raw" />
+        <remap from="input/camera_info" to="camera_info" />
+        <remap from="output/camera_info" to="optical/camera_info" />
+      </node>
+
       <node
         pkg="ros_ign_bridge"
         type="parameter_bridge"
@@ -143,6 +196,16 @@
         args="$(arg sensor_prefix)/rear_realsense/depth_image">
         <remap from="$(arg sensor_prefix)/rear_realsense/depth_image" to="depth/image_raw"/>
       </node>
+
+      <node
+        pkg="subt_ros"
+        type="optical_frame_publisher"
+        name="optical_frame_publisher_depth"
+        args="depth">
+        <remap from="input/image" to="depth/image_raw" />
+        <remap from="output/image" to="depth/optical/image_raw" />
+      </node>
+
     </group>
 
     <!--Right RGBD camera -->
@@ -161,6 +224,17 @@
         args="$(arg sensor_prefix)/right_realsense/image">
         <remap from="$(arg sensor_prefix)/right_realsense/image" to="image_raw"/>
       </node>
+
+      <node
+        pkg="subt_ros"
+        type="optical_frame_publisher"
+        name="optical_frame_publisher">
+        <remap from="input/image" to="image_raw" />
+        <remap from="output/image" to="optical/image_raw" />
+        <remap from="input/camera_info" to="camera_info" />
+        <remap from="output/camera_info" to="optical/camera_info" />
+      </node>
+
       <node
         pkg="ros_ign_bridge"
         type="parameter_bridge"
@@ -175,6 +249,16 @@
         args="$(arg sensor_prefix)/right_realsense/depth_image">
         <remap from="$(arg sensor_prefix)/right_realsense/depth_image" to="depth/image_raw"/>
       </node>
+
+      <node
+        pkg="subt_ros"
+        type="optical_frame_publisher"
+        name="optical_frame_publisher_depth"
+        args="depth">
+        <remap from="input/image" to="depth/image_raw" />
+        <remap from="output/image" to="depth/optical/image_raw" />
+      </node>
+
     </group>
 
     <!--Front Laser-->

diff --git a/submitted_models/robotika_x2_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/robotika_x2_sensor_config_1/launch/vehicle_topics.launch
@@ -77,6 +77,17 @@
       args="$(arg sensor_prefix)/camera_front/image">
       <remap from="$(arg sensor_prefix)/camera_front/image" to="image_raw"/>
     </node>
+
+    <node
+      pkg="subt_ros"
+      type="optical_frame_publisher"
+      name="optical_frame_publisher">
+      <remap from="input/image" to="image_raw" />
+      <remap from="output/image" to="optical/image_raw" />
+      <remap from="input/camera_info" to="camera_info" />
+      <remap from="output/camera_info" to="optical/camera_info" />
+    </node>
+
     <node
       pkg="ros_ign_bridge"
       type="parameter_bridge"
@@ -91,7 +102,16 @@
       args="$(arg sensor_prefix)/camera_front/depth_image">
       <remap from="$(arg sensor_prefix)/camera_front/depth_image" to="front/depth"/>
     </node>
-
+
+    <node
+      pkg="subt_ros"
+      type="optical_frame_publisher"
+      name="optical_frame_publisher_depth"
+      args="depth">
+      <remap from="input/image" to="front/depth" />
+      <remap from="output/image" to="front/optical/depth" />
+    </node>
+
     <!-- Gas sensor -->
     <node
       pkg="ros_ign_bridge"

diff --git a/submitted_models/sophisticated_engineering_x2_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/sophisticated_engineering_x2_sensor_config_1/launch/vehicle_topics.launch
@@ -71,8 +71,25 @@
         <remap from="$(arg sensor_prefix)/camera_front_left/image" to="front/left/image_raw"/>
       </node>
 
+      <node
+        pkg="subt_ros"
+        type="optical_frame_publisher"
+        name="optical_frame_publisher_stereo_right">
+        <remap from="input/image" to="front/right/image_raw" />
+        <remap from="output/image" to="front/right/optical/image_raw" />
+        <remap from="input/camera_info" to="front/right/camera_info" />
+        <remap from="output/camera_info" to="front/right/optical/camera_info" />
+      </node>
 
-
+      <node
+        pkg="subt_ros"
+        type="optical_frame_publisher"
+        name="optical_frame_publisher_stereo_left">
+        <remap from="input/image" to="front/left/image_raw" />
+        <remap from="output/image" to="front/left/optical/image_raw" />
+        <remap from="input/camera_info" to="front/left/camera_info" />
+        <remap from="output/camera_info" to="front/left/optical/camera_info" />
+      </node>
 
 
     <!-- UGV -->

diff --git a/submitted_models/sophisticated_engineering_x4_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/sophisticated_engineering_x4_sensor_config_1/launch/vehicle_topics.launch
@@ -77,8 +77,25 @@
         <remap from="$(arg sensor_prefix)/camera_front_left/image" to="front/left/image_raw"/>
       </node>
 
+      <node
+        pkg="subt_ros"
+        type="optical_frame_publisher"
+        name="optical_frame_publisher_stereo_right">
+        <remap from="input/image" to="front/right/image_raw" />
+        <remap from="output/image" to="front/right/optical/image_raw" />
+        <remap from="input/camera_info" to="front/right/camera_info" />
+        <remap from="output/camera_info" to="front/right/optical/camera_info" />
+      </node>
 
-
+      <node
+        pkg="subt_ros"
+        type="optical_frame_publisher"
+        name="optical_frame_publisher_stereo_left">
+        <remap from="input/image" to="front/left/image_raw" />
+        <remap from="output/image" to="front/left/optical/image_raw" />
+        <remap from="input/camera_info" to="front/left/camera_info" />
+        <remap from="output/camera_info" to="front/left/optical/camera_info" />
+      </node>
 
     <!-- UAV -->
 

diff --git a/submitted_models/ssci_x2_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/ssci_x2_sensor_config_1/launch/vehicle_topics.launch
@@ -70,6 +70,16 @@
         args="$(arg sensor_prefix)/camera_front/depth_image">
         <remap from="$(arg sensor_prefix)/camera_front/depth_image" to="front/depth"/>
       </node>
+
+      <node
+        pkg="subt_ros"
+        type="optical_frame_publisher"
+        name="optical_frame_publisher_depth"
+        args="depth">
+        <remap from="input/image" to="front/depth" />
+        <remap from="output/image" to="front/optical/depth" />
+      </node>
+
       <node
         pkg="ros_ign_bridge"
         type="parameter_bridge"
@@ -84,6 +94,17 @@
         args="$(arg sensor_prefix)/camera_front/image">
         <remap from="$(arg sensor_prefix)/camera_front/image" to="front/image_raw"/>
       </node>
+
+      <node
+        pkg="subt_ros"
+        type="optical_frame_publisher"
+        name="optical_frame_publisher">
+        <remap from="input/image" to="front/image_raw" />
+        <remap from="output/image" to="front/optical/image_raw" />
+        <remap from="input/camera_info" to="front/camera_info" />
+        <remap from="output/camera_info" to="front/optical/camera_info" />
+      </node>
+
     </group>
 
     <group if="$(arg rgbd_cam_up)">
@@ -101,6 +122,16 @@
         args="$(arg sensor_prefix)/camera_up/depth_image">
         <remap from="$(arg sensor_prefix)/camera_up/depth_image" to="up/depth"/>
       </node>
+
+      <node
+        pkg="subt_ros"
+        type="optical_frame_publisher"
+        name="optical_frame_publisher_depth_up"
+        args="depth">
+        <remap from="input/image" to="up/depth" />
+        <remap from="output/image" to="up/optical/depth" />
+      </node>
+
       <node
         pkg="ros_ign_bridge"
         type="parameter_bridge"
@@ -115,6 +146,17 @@
         args="$(arg sensor_prefix)/camera_up/image">
         <remap from="$(arg sensor_prefix)/camera_up/image" to="up/image_raw"/>
       </node>
+
+      <node
+        pkg="subt_ros"
+        type="optical_frame_publisher"
+        name="optical_frame_publisher_up">
+        <remap from="input/image" to="up/image_raw" />
+        <remap from="output/image" to="up/optical/image_raw" />
+        <remap from="input/camera_info" to="up/camera_info" />
+        <remap from="output/camera_info" to="up/optical/camera_info" />
+      </node>
+
     </group>
 
     <group if="$(arg rgbd_cam_down)">
@@ -132,6 +174,16 @@
         args="$(arg sensor_prefix)/camera_down/depth_image">
         <remap from="$(arg sensor_prefix)/camera_down/depth_image" to="down/depth"/>
       </node>
+
+      <node
+        pkg="subt_ros"
+        type="optical_frame_publisher"
+        name="optical_frame_publisher_depth_down"
+        args="depth">
+        <remap from="input/image" to="down/depth" />
+        <remap from="output/image" to="down/optical/depth" />
+      </node>
+
       <node
         pkg="ros_ign_bridge"
         type="parameter_bridge"
@@ -146,6 +198,17 @@
         args="$(arg sensor_prefix)/camera_down/image">
         <remap from="$(arg sensor_prefix)/camera_down/image" to="down/image_raw"/>
       </node>
+
+      <node
+        pkg="subt_ros"
+        type="optical_frame_publisher"
+        name="optical_frame_publisher_down">
+        <remap from="input/image" to="down/image_raw" />
+        <remap from="output/image" to="down/optical/image_raw" />
+        <remap from="input/camera_info" to="down/camera_info" />
+        <remap from="output/camera_info" to="down/optical/camera_info" />
+      </node>
+
     </group>
 
     <!-- 3D lidar -->
@@ -179,6 +242,26 @@
         <remap from="$(arg sensor_prefix)/camera_front_right/image" to="front/right/image_raw"/>
         <remap from="$(arg sensor_prefix)/camera_front_left/image" to="front/left/image_raw"/>
       </node>
+
+      <node
+        pkg="subt_ros"
+        type="optical_frame_publisher"
+        name="optical_frame_publisher_stereo_right">
+        <remap from="input/image" to="front/right/image_raw" />
+        <remap from="output/image" to="front/right/optical/image_raw" />
+        <remap from="input/camera_info" to="front/right/camera_info" />
+        <remap from="output/camera_info" to="front/right/optical/camera_info" />
+      </node>
+
+      <node
+        pkg="subt_ros"
+        type="optical_frame_publisher"
+        name="optical_frame_publisher_stereo_left">
+        <remap from="input/image" to="front/left/image_raw" />
+        <remap from="output/image" to="front/left/optical/image_raw" />
+        <remap from="input/camera_info" to="front/left/camera_info" />
+        <remap from="output/camera_info" to="front/left/optical/camera_info" />
+      </node>
     </group>
 
     <!-- laser scan -->