Add depth camera model for ROS2 environment (#707)

* add jinja template for depth_camera in ros/ros2 environment * modify parameters and frame name for ros2
PX4 · Feb 10, 2021 · bebb9a9 · bebb9a9
1 parent 54e1a19
commit bebb9a9
Show file tree

Hide file tree

Showing 3 changed files with 48 additions and 1 deletion.
diff --git a/.gitignore b/.gitignore
@@ -26,3 +26,4 @@ models/rover/rover.sdf
 models/tiltrotor/tiltrotor.sdf
 models/boat/boat.sdf
 models/typhoon_h480/typhoon_h480.sdf
+models/depth_camera/depth_camera.sdf
diff --git a/models/depth_camera/depth_camera.sdf → models/depth_camera/depth_camera.sdf.jinja b/models/depth_camera/depth_camera.sdf → models/depth_camera/depth_camera.sdf.jinja
@@ -1,3 +1,4 @@
+<!-- DO NOT EDIT: Generated from depth_camera.sdf.jinja -->
 <?xml version="1.0" ?>
 <sdf version="1.5">
   <model name="depth_camera">
@@ -24,6 +25,38 @@
         </geometry>
       </visual>
       <sensor name="depth_camera" type="depth">
+        {% if ros2_distro in ['dashing', 'eloquent', 'foxy'] %}
+        <always_on>0</always_on>
+        <update_rate>20</update_rate>
+        <camera name="camera_name">
+          <horizontal_fov>1.02974</horizontal_fov>
+          <image>
+            <format>R8G8B8</format>
+            <width>64</width>
+            <height>48</height>
+          </image>
+          <clip>
+            <near>0.5</near>
+            <far>18</far>
+          </clip>
+          <distortion>
+            <k1>0.0</k1>
+            <k2>0.0</k2>
+            <k3>0.0</k3>
+            <p1>0.0</p1>
+            <p2>0.0</p2>
+            <center>0.5 0.5</center>
+          </distortion>
+        </camera>
+
+        <plugin name="camera_controller" filename="libgazebo_ros_camera.so">
+          <camera_name>camera</camera_name>
+          <frame_name>camera_link</frame_name>
+          <hack_baseline>0.07</hack_baseline>
+          <min_depth>0.1</min_depth>
+          <max_depth>12</max_depth>
+        </plugin>
+        {% else %}
         <update_rate>20</update_rate>
         <camera>
           <horizontal_fov>1.02974</horizontal_fov>
@@ -55,6 +88,7 @@
           <distortion_t1>0.0</distortion_t1>
           <distortion_t2>0.0</distortion_t2>
         </plugin>
+        {% endif %}
       </sensor>
     </link>
   </model>

diff --git a/scripts/jinja_gen.py b/scripts/jinja_gen.py
@@ -31,10 +31,21 @@ def get_file_contents(filepath):
     parser.add_argument('--gst_udp_port', default=5600, help="Gstreamer UDP port for SITL")
     parser.add_argument('--video_uri', default=5600, help="Mavlink camera URI for SITL")
     parser.add_argument('--mavlink_cam_udp_port', default=14530, help="Mavlink camera UDP port for SITL")
+    parser.add_argument('--ros2-distro', default='', dest='ros2_distro', type=str,
+                    help="ROS2 distro, only required if generating the agent for usage with ROS2 nodes, by default empty")
     args = parser.parse_args()
     env = jinja2.Environment(loader=jinja2.FileSystemLoader(args.env_dir))
     template = env.get_template(os.path.relpath(args.filename, args.env_dir))
 
+    # get ROS 2 version, if exists
+    ros2_distro = ''
+    ros_version = os.environ.get('ROS_VERSION')
+    if ros_version == '2':
+        if args.ros2_distro != '':
+            ros2_distro = args.ros2_distro
+        else:
+            ros2_distro = os.environ.get('ROS_DISTRO')
+
     # create dictionary with useful modules etc.
     try:
         import rospkg
@@ -54,7 +65,8 @@ def get_file_contents(filepath):
          'gst_udp_port': args.gst_udp_port, \
          'video_uri': args.video_uri, \
          'mavlink_cam_udp_port': args.mavlink_cam_udp_port, \
-         'hil_mode': args.hil_mode}
+         'hil_mode': args.hil_mode, \
+         'ros2_distro': ros2_distro}
 
     result = template.render(d)