From ad6b1149e8fe342f22a86848bc3eedd97aebdfaa Mon Sep 17 00:00:00 2001
From: "Pasch, Frederik" <frederik.pasch@intel.com>
Date: Fri, 30 Aug 2024 16:38:26 +0200
Subject: [PATCH] gazebo_static_camera: allow setting resolution and
 update-rate

---
 .../launch/spawn_static_camera_launch.py      | 23 ++++++++++++++++---
 .../models/{camera.sdf => camera.sdf.xacro}   | 12 ++++++----
 2 files changed, 28 insertions(+), 7 deletions(-)
 rename simulation/gazebo/gazebo_static_camera/models/{camera.sdf => camera.sdf.xacro} (75%)

diff --git a/simulation/gazebo/gazebo_static_camera/launch/spawn_static_camera_launch.py b/simulation/gazebo/gazebo_static_camera/launch/spawn_static_camera_launch.py
index 3a248ff8..407716e6 100644
--- a/simulation/gazebo/gazebo_static_camera/launch/spawn_static_camera_launch.py
+++ b/simulation/gazebo/gazebo_static_camera/launch/spawn_static_camera_launch.py
@@ -13,11 +13,11 @@
 # and limitations under the License.
 #
 # SPDX-License-Identifier: Apache-2.0
-
+import tempfile
 from ament_index_python.packages import get_package_share_directory
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument
+from launch.actions import DeclareLaunchArgument, ExecuteProcess
 from launch.substitutions import LaunchConfiguration
 from launch.substitutions.path_join_substitution import PathJoinSubstitution
 
@@ -29,6 +29,15 @@
 
     DeclareLaunchArgument('world_name', default_value='default',
                           description='World name'),
+
+    DeclareLaunchArgument('update_rate', default_value='5',
+                          description='Update rate of the sensor'),
+
+    DeclareLaunchArgument('image_width', default_value='320',
+                          description='Width of camera image'),
+
+    DeclareLaunchArgument('image_height', default_value='240',
+                          description='Height of camera image'),
 ]
 
 for pose_element in ['x', 'y', 'z', 'roll', 'pitch', 'yaw']:
@@ -41,10 +50,17 @@ def generate_launch_description():
 
     camera_name = LaunchConfiguration('camera_name')
     world_name = LaunchConfiguration('world_name')
+    update_rate = LaunchConfiguration('update_rate')
+    image_width = LaunchConfiguration('image_width')
+    image_height = LaunchConfiguration('image_height')
+    camera_sdf = tempfile.NamedTemporaryFile(prefix='gazebo_static_camera_', suffix='.sdf', delete=False)
 
     x, y, z = LaunchConfiguration('x'), LaunchConfiguration('y'), LaunchConfiguration('z')
     roll, pitch, yaw = LaunchConfiguration('roll'), LaunchConfiguration('pitch'), LaunchConfiguration('yaw')
 
+    camera_sdf_xacro = ExecuteProcess(
+        cmd=['xacro', '-o', camera_sdf.name, ['update_rate:=', update_rate], ['image_width:=', image_width], ['image_height:=', image_height], PathJoinSubstitution([gazebo_static_camera_dir, 'models', 'camera.sdf.xacro'])])
+
     camera_bridge = Node(
         package='ros_gz_bridge',
         executable='parameter_bridge',
@@ -80,11 +96,12 @@ def generate_launch_description():
                    '-R', roll,
                    '-P', pitch,
                    '-Y', yaw,
-                   '-file', PathJoinSubstitution([gazebo_static_camera_dir, 'models', 'camera.sdf'])],
+                   '-file', camera_sdf.name],
         output='screen'
     )
 
     ld = LaunchDescription(ARGUMENTS)
+    ld.add_action(camera_sdf_xacro)
     ld.add_action(camera_bridge)
     ld.add_action(spawn_camera)
     return ld
diff --git a/simulation/gazebo/gazebo_static_camera/models/camera.sdf b/simulation/gazebo/gazebo_static_camera/models/camera.sdf.xacro
similarity index 75%
rename from simulation/gazebo/gazebo_static_camera/models/camera.sdf
rename to simulation/gazebo/gazebo_static_camera/models/camera.sdf.xacro
index 32b945e3..28ee7986 100644
--- a/simulation/gazebo/gazebo_static_camera/models/camera.sdf
+++ b/simulation/gazebo/gazebo_static_camera/models/camera.sdf.xacro
@@ -1,5 +1,9 @@
 <?xml version='1.0'?>
-<sdf version="1.4">
+<sdf version="1.4" xmlns:xacro="http://ros.org/wiki/xacro">
+  <xacro:arg name="image_width" default="320"/>
+  <xacro:arg name="image_height" default="240"/>
+  <xacro:arg name="update_rate" default="5"/>
+  
   <model name="camera">
     <static>true</static>
       <link name="link">
@@ -30,8 +34,8 @@
           <camera>
             <horizontal_fov>1.047</horizontal_fov>
             <image>
-              <width>320</width>
-              <height>240</height>
+              <width>$(arg image_width)</width>
+              <height>$(arg image_height)</height>
             </image>
             <clip>
               <near>0.1</near>
@@ -39,7 +43,7 @@
             </clip>
           </camera>
           <always_on>1</always_on>
-          <update_rate>5</update_rate>
+          <update_rate>$(arg update_rate)</update_rate>
           <visualize>true</visualize>
         </sensor>
       </link>