Add option to rotate Gazebo camera image

stretch_gazebo/launch/gazebo.launch

@@ -11,6 +11,7 @@
   <arg name="world" default="worlds/empty.world"/>
   <arg name="dex_wrist" default="false"/>
   <param name="stretch_gazebo/dex_wrist" type="bool" value="$(arg dex_wrist)"/>
+  <arg name="publish_upright_img" default="false" doc="whether to pub rotated upright color image" />
 
   <arg name="model" value="$(find stretch_gazebo)/urdf/stretch_gazebo_standard_gripper.urdf.xacro" unless="$(arg dex_wrist)"/>
   <arg name="model" value="$(find stretch_gazebo)/urdf/stretch_gazebo_dex_wrist.urdf.xacro" if="$(arg dex_wrist)"/>
@@ -39,7 +40,7 @@
   <node name="rviz" pkg="rviz" type="rviz" args="-d $(find stretch_gazebo)/config/sim.rviz" if="$(arg rviz)"/>
 
   <rosparam command="load"
-            file="$(find stretch_gazebo)/config/joints.yaml" 
+            file="$(find stretch_gazebo)/config/joints.yaml"
             ns="stretch_joint_state_controller" />
 
   <rosparam command="load"
@@ -68,4 +69,12 @@
 
   <node name="publish_ground_truth_odom" pkg="stretch_gazebo" type="publish_ground_truth_odom.py" output="screen"/>
 
+  <!-- UPRIGHT ROTATED CAMERA VIEW -->
+  <node name="upright_rotater" pkg="image_rotate" type="image_rotate" if="$(arg publish_upright_img)">
+    <remap from="image" to="/camera/color/image_raw" />
+    <remap from="rotated/image" to="/camera/color/upright_image_raw" />
+    <param name="target_frame_id" type="str" value="" />
+    <param name="target_x" type="double" value="-1.5708" />
+  </node>
+
 </launch>