Add missing changes from PRs #402 and #403

Signed-off-by: Addisu Z. Taddese <[email protected]>
osrf · May 20, 2020 · e470e98 · e470e98
1 parent f27805e
commit e470e98
Show file tree

Hide file tree

Showing 4 changed files with 126 additions and 3 deletions.
diff --git a/submitted_models/costar_husky_sensor_config_2/launch/spawner.rb b/submitted_models/costar_husky_sensor_config_2/launch/spawner.rb
@@ -29,6 +29,9 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw)
         <publish_collision_pose>false</publish_collision_pose>
         <publish_visual_pose>false</publish_visual_pose>
         <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>
+        <use_pose_vector_msg>true</use_pose_vector_msg>
+        <static_publisher>true</static_publisher>
+        <static_update_frequency>1</static_update_frequency>
       </plugin>
       <!-- Battery plugin -->
       <plugin filename="libignition-gazebo-linearbatteryplugin-system.so"

diff --git a/submitted_models/costar_husky_sensor_config_2/launch/vehicle_topics.launch b/submitted_models/costar_husky_sensor_config_2/launch/vehicle_topics.launch
@@ -10,9 +10,16 @@
       pkg="ros_ign_bridge"
       type="parameter_bridge"
       name="ros_ign_bridge_pose"
-      args="/model/$(arg name)/pose@geometry_msgs/TransformStamped[ignition.msgs.Pose">
+      args="/model/$(arg name)/pose@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
       <remap from="/model/$(arg name)/pose" to="pose"/>
     </node>
+    <node
+      pkg="ros_ign_bridge"
+      type="parameter_bridge"
+      name="ros_ign_bridge_pose_static"
+      args="/model/$(arg name)/pose_static@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
+      <remap from="/model/$(arg name)/pose_static" to="pose_static"/>
+    </node>
 
     <node
       pkg="subt_ros"
@@ -58,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"
@@ -73,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_2/launch/spawner.rb b/submitted_models/explorer_x1_sensor_config_2/launch/spawner.rb
@@ -29,6 +29,9 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw)
         <publish_collision_pose>false</publish_collision_pose>
         <publish_visual_pose>false</publish_visual_pose>
         <publish_nested_model_pose>#{$enableGroundTruth}</publish_nested_model_pose>
+        <use_pose_vector_msg>true</use_pose_vector_msg>
+        <static_publisher>true</static_publisher>
+        <static_update_frequency>1</static_update_frequency>
       </plugin>
       <!-- Battery plugin -->
       <plugin filename="libignition-gazebo-linearbatteryplugin-system.so"

diff --git a/submitted_models/explorer_x1_sensor_config_2/launch/vehicle_topics.launch b/submitted_models/explorer_x1_sensor_config_2/launch/vehicle_topics.launch
@@ -10,10 +10,16 @@
       pkg="ros_ign_bridge"
       type="parameter_bridge"
       name="ros_ign_bridge_pose"
-      args="/model/$(arg name)/pose@geometry_msgs/TransformStamped[ignition.msgs.Pose">
+      args="/model/$(arg name)/pose@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
       <remap from="/model/$(arg name)/pose" to="pose"/>
     </node>
-
+    <node
+      pkg="ros_ign_bridge"
+      type="parameter_bridge"
+      name="ros_ign_bridge_pose_static"
+      args="/model/$(arg name)/pose_static@tf2_msgs/TFMessage[ignition.msgs.Pose_V">
+      <remap from="/model/$(arg name)/pose_static" to="pose_static"/>
+    </node>
     <node
       pkg="subt_ros"
       type="pose_tf_broadcaster"
@@ -59,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"
@@ -73,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 -->
@@ -91,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"
@@ -105,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 -->
@@ -123,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"
@@ -137,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 -->
@@ -155,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"
@@ -169,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-->