From 2671475e8461757a21a92a97673d11f56f616d28 Mon Sep 17 00:00:00 2001
From: Binit Shah <bshah@hello-robot.com>
Date: Wed, 13 Sep 2023 23:23:32 -0400
Subject: [PATCH] Add option to rotate Gazebo camera image

---
 stretch_gazebo/launch/gazebo.launch | 11 ++++++++++-
 1 file changed, 10 insertions(+), 1 deletion(-)

diff --git a/stretch_gazebo/launch/gazebo.launch b/stretch_gazebo/launch/gazebo.launch
index f479e09..0cfba9a 100644
--- a/stretch_gazebo/launch/gazebo.launch
+++ b/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>