gazebo_static_camera: allow setting resolution and update-rate

IntelLabs · Aug 30, 2024 · e9d00ef · e9d00ef
1 parent f18623f
commit e9d00ef
Show file tree

Hide file tree

Showing 3 changed files with 29 additions and 7 deletions.
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
@@ -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/...bo/gazebo_static_camera/models/camera.sdf → ...ebo_static_camera/models/camera.sdf.xacro b/...bo/gazebo_static_camera/models/camera.sdf → ...ebo_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,16 +34,16 @@
           <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>
               <far>100</far>
             </clip>
           </camera>
           <always_on>1</always_on>
-          <update_rate>5</update_rate>
+          <update_rate>$(arg update_rate)</update_rate>
           <visualize>true</visualize>
         </sensor>
       </link>

diff --git a/simulation/gazebo/gazebo_static_camera/package.xml b/simulation/gazebo/gazebo_static_camera/package.xml
@@ -11,6 +11,7 @@
   <exec_depend>rclpy</exec_depend>
   <exec_depend>ros_gz_bridge</exec_depend>
   <exec_depend>ros_ign_gazebo</exec_depend>
+  <exec_depend>xacro</exec_depend>
 
   <test_depend>ament_copyright</test_depend>
   <test_depend>ament_flake8</test_depend>