From 65acd3c8ec023a8ca15d704655f38f566d97c953 Mon Sep 17 00:00:00 2001
From: bfotheri <fotheringhambrett@gmail.com>
Date: Thu, 18 Jun 2020 08:56:04 -0600
Subject: [PATCH] Updated branch based on recommended PR changes (#464)
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

* Made changes based on pull request comments

* Update spawner.rb to fix negative accel limit

* updated instructions for building ros ign bridge

* Update spawner.rb

Co-authored-by: Carlos Agüero <cen.aguero@gmail.com>
---
 .../BuildingCustomRosIgnBridgeForMarble.txt   | 11 ++--
 .../launch/spawner.rb                         |  4 ++
 .../launch/vehicle_topics.launch              | 51 +++++++++++++++++++
 subt_ros/launch/competition_init.launch       |  4 +-
 4 files changed, 62 insertions(+), 8 deletions(-)

diff --git a/submitted_models/marble_husky_sensor_config_1/BuildingCustomRosIgnBridgeForMarble.txt b/submitted_models/marble_husky_sensor_config_1/BuildingCustomRosIgnBridgeForMarble.txt
index d7ac505c..2af1b577 100644
--- a/submitted_models/marble_husky_sensor_config_1/BuildingCustomRosIgnBridgeForMarble.txt
+++ b/submitted_models/marble_husky_sensor_config_1/BuildingCustomRosIgnBridgeForMarble.txt
@@ -1,10 +1,10 @@
-# Building custom ros_ign_bridge to make pan/tilt gimbal work (on marble vehicles):
+# Building ros_ign_bridge to make pan/tilt gimbal work (on marble vehicles):
 
 First, follow instructions for building from source on this page:
-https://github.com/realdealneil/ros_ign/tree/melodic/ros_ign_bridge
+https://github.com/osrf/ros_ign/tree/melodic/ros_ign_bridge
 
 Except, use the following clone URL instead:
-git clone https://github.com/realdealneil/ros_ign.git -b melodic
+git clone https://github.com/osrf/ros_ign.git -b melodic
 
 Once you've built, you will need to stack your workspace (subt workspace)
 on top of this workspace.  To do this, do as follows:
@@ -17,6 +17,5 @@ source ~/bridge_ws/install/setup.bash
 catkin_make install
 ```
 
-Alternatively, you can clone the above repo directly inside of subt repo 
-and do a clean build.  I have issued pull request on ros_ign_bridge and
-once that has made its way into subt, this will no longer be necessary.  
+Once the binaries for this package are updated a simple "sudo apt-get install ros-melodic-ros-ign-bridge" will suffice for installation
+
diff --git a/submitted_models/marble_husky_sensor_config_1/launch/spawner.rb b/submitted_models/marble_husky_sensor_config_1/launch/spawner.rb
index 41d44932..bbedab7a 100644
--- a/submitted_models/marble_husky_sensor_config_1/launch/spawner.rb
+++ b/submitted_models/marble_husky_sensor_config_1/launch/spawner.rb
@@ -20,6 +20,10 @@ def spawner(_name, _modelURI, _worldName, _x, _y, _z, _roll, _pitch, _yaw)
         <wheel_separation>#{0.45649 * 1.5}</wheel_separation>
         <wheel_radius>0.1651</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_husky_sensor_config_1/launch/vehicle_topics.launch b/submitted_models/marble_husky_sensor_config_1/launch/vehicle_topics.launch
index 4586f2eb..b49f8901 100644
--- a/submitted_models/marble_husky_sensor_config_1/launch/vehicle_topics.launch
+++ b/submitted_models/marble_husky_sensor_config_1/launch/vehicle_topics.launch
@@ -107,6 +107,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
@@ -137,6 +154,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
@@ -167,6 +201,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