From 657631b9038342b2aadd522ef25b505a1b59cb87 Mon Sep 17 00:00:00 2001
From: bfotheri <fotheringhambrett@gmail.com>
Date: Mon, 15 Jun 2020 09:44:29 -0600
Subject: [PATCH] Updated vehicle params based on PR feedback (depth topics,
 vel limits, uncommented logger

---
 .../launch/spawner.rb                         |  4 ++
 .../launch/vehicle_topics.launch              | 51 +++++++++++++++++++
 subt_ros/launch/competition_init.launch       |  4 +-
 3 files changed, 57 insertions(+), 2 deletions(-)

diff --git a/submitted_models/marble_hd2_sensor_config_1/launch/spawner.rb b/submitted_models/marble_hd2_sensor_config_1/launch/spawner.rb
index efc79e94..2909820b 100644
--- a/submitted_models/marble_hd2_sensor_config_1/launch/spawner.rb
+++ b/submitted_models/marble_hd2_sensor_config_1/launch/spawner.rb
@@ -24,6 +24,10 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw)
         <wheel_separation>#{0.525}</wheel_separation>
         <wheel_radius>0.129</wheel_radius>
         <topic>/model/#{_name}/cmd_vel_relay</topic>
+        <min_velocity>-1</min_velocity>
+        <max_velocity>1</max_velocity>
+        <min_acceleration>-3</min_acceleration>
+        <max_acceleration>3</max_acceleration>
       </plugin>
       <!-- Publish robot state information -->
       <plugin filename="libignition-gazebo-pose-publisher-system.so"
diff --git a/submitted_models/marble_hd2_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/marble_hd2_sensor_config_1/launch/vehicle_topics.launch
index bf032c45..6c15fa36 100644
--- a/submitted_models/marble_hd2_sensor_config_1/launch/vehicle_topics.launch
+++ b/submitted_models/marble_hd2_sensor_config_1/launch/vehicle_topics.launch
@@ -105,6 +105,23 @@
       args="$(arg sensor_prefix)/camera_front/depth_image">
       <remap from="$(arg sensor_prefix)/camera_front/depth_image" to="front_facing/depth"/>
     </node>
+    <node
+      pkg="subt_ros"
+      type="optical_frame_publisher"
+      name="optical_frame_publisher_image_front_facing">
+      <remap from="input/image" to="front_facing/image_raw" />
+      <remap from="output/image" to="front_facing/optical/image_raw" />
+      <remap from="input/camera_info" to="front_facing/camera_info" />
+      <remap from="output/camera_info" to="front_facing/optical/camera_info" />
+    </node>
+    <node
+      pkg="subt_ros"
+      type="optical_frame_publisher"
+      name="optical_frame_publisher_depth_front"
+      args="depth">
+      <remap from="input/image" to="front_facing/depth" />
+      <remap from="output/image" to="front_facing/optical/depth" />
+    </node>
 
     <!--30 Degree downward looking RGBD camera -->
     <node
@@ -135,6 +152,23 @@
       args="$(arg sensor_prefix)/camera_down/depth_image">
       <remap from="$(arg sensor_prefix)/camera_down/depth_image" to="front_down/depth"/>
     </node>
+    <node
+      pkg="subt_ros"
+      type="optical_frame_publisher"
+      name="optical_frame_publisher_image_front_down">
+      <remap from="input/image" to="front_down/image_raw" />
+      <remap from="output/image" to="front_down/optical/image_raw" />
+      <remap from="input/camera_info" to="front_down/camera_info" />
+      <remap from="output/camera_info" to="front_down/optical/camera_info" />
+    </node>
+    <node
+      pkg="subt_ros"
+      type="optical_frame_publisher"
+      name="optical_frame_publisher_depth_down"
+      args="depth">
+      <remap from="input/image" to="front_down/depth" />
+      <remap from="output/image" to="front_down/optical/depth" />
+    </node>
 
       <!--Pan Tilt RGBD camera -->
     <node
@@ -165,6 +199,23 @@
       args="$(arg gimbal_sensor_prefix)/camera_pan_tilt/depth_image">
       <remap from="$(arg gimbal_sensor_prefix)/camera_pan_tilt/depth_image" to="pan_tilt/depth"/>
     </node>
+    <node
+      pkg="subt_ros"
+      type="optical_frame_publisher"
+      name="optical_frame_publisher_image_pan_tilt">
+      <remap from="input/image" to="pan_tilt/image_raw" />
+      <remap from="output/image" to="pan_tilt/optical/image_raw" />
+      <remap from="input/camera_info" to="pan_tilt/camera_info" />
+      <remap from="output/camera_info" to="pan_tilt/optical/camera_info" />
+    </node>
+    <node
+      pkg="subt_ros"
+      type="optical_frame_publisher"
+      name="optical_frame_publisher_depth_pan_tilt"
+      args="depth">
+      <remap from="input/image" to="pan_tilt/depth" />
+      <remap from="output/image" to="pan_tilt/optical/depth" />
+    </node>
 
     <!--OS1-64 3D Laser-->
     <node
diff --git a/subt_ros/launch/competition_init.launch b/subt_ros/launch/competition_init.launch
index 75558e93..5b23bbe8 100644
--- a/subt_ros/launch/competition_init.launch
+++ b/subt_ros/launch/competition_init.launch
@@ -18,10 +18,10 @@
     type="subt_ros_relay"
     name="subt_ros_relay"/>
 
-  <!--<node
+  <node
     pkg="subt_ros"
     type="bridge_logger"
-    name="bridge_logger"/>-->
+    name="bridge_logger"/>
 
 
   <node