make depth camera config match zed URDF / config

src/description/urdf/athena_drive.urdf.xacro

@@ -15,7 +15,7 @@
   <xacro:include filename="imu.xacro"/>
   <xacro:include filename="gps.xacro"/>
   <xacro:include filename="ground_truth_odom.xacro"/>
-  <xacro:include filename="depth_camera.xacro"/> 
+  <xacro:include filename="zed_camera.xacro"/>
 
 
   <!-- Load robot's macro with parameters -->
@@ -56,9 +56,9 @@
 
   <xacro:ground_truth_odometry robot_base_frame="base_footprint"/>
 
-  <xacro:depth_camera parent="base_footprint"> 
-      <origin xyz=".5 0.1 0.1" rpy="0 0 0"/>
-  </xacro:depth_camera>
+  <xacro:zed_camera_sensor parent="base_footprint" camera_name="zed" camera_model="zed2i" sim_gazebo="$(arg sim_gazebo)">
+    <origin xyz=".5 0.1 0.1" rpy="0 0 0"/>
+  </xacro:zed_camera_sensor>
 
   <xacro:if value="$(arg use_mock_hardware)">
     <xacro:athena_drive_ros2_control

src/description/urdf/zed_camera.xacro

@@ -0,0 +1,57 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:include filename="$(find zed_wrapper)/urdf/zed_macro.urdf.xacro"/>
+
+  <!--
+    Parameters:
+      parent       - parent link to attach the camera to
+      camera_name  - camera name used for all link/joint/topic names (default: zed)
+      camera_model - ZED model: zed, zedm, zed2, zed2i, zedx, zedxm, ... (default: zed2i)
+      sim_gazebo   - when true, adds a Gazebo depth sensor on the left camera frame (default: false)
+      *origin      - transform from parent to the camera mounting screw point
+  -->
+  <xacro:macro name="zed_camera_sensor" params="parent camera_name:=zed camera_model:=zed2i sim_gazebo:=false *origin">
+
+    <joint name="${camera_name}_mount_joint" type="fixed">
+      <xacro:insert_block name="origin"/>
+      <parent link="${parent}"/>
+      <child link="${camera_name}_camera_link"/>
+    </joint>
+
+    <xacro:zed_camera name="${camera_name}" model="${camera_model}" enable_gnss="false">
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+    </xacro:zed_camera>
+
+    <xacro:if value="${sim_gazebo}">
+      <gazebo reference="${camera_name}_left_camera_frame">
+        <sensor name="${camera_name}_depth_sensor" type="depth">
+          <always_on>1</always_on>
+          <update_rate>15</update_rate>
+          <visualize>true</visualize>
+          <topic>${camera_name}/zed_node/point_cloud/cloud_registered</topic>
+          <ign_frame_id>${camera_name}_left_camera_frame_optical</ign_frame_id>
+          <camera name="${camera_name}_depth">
+            <horizontal_fov>1.8151</horizontal_fov>
+            <image>
+              <width>672</width>
+              <height>376</height>
+              <format>R_FLOAT32</format>
+            </image>
+            <clip>
+              <near>0.2</near>
+              <far>20.0</far>
+            </clip>
+            <depth_camera>
+              <output>points</output>
+            </depth_camera>
+          </camera>
+          <plugin filename="libignition-gazebo-sensors-system.so" name="ignition::gazebo::systems::Sensors">
+            <render_engine>ogre2</render_engine>
+          </plugin>
+        </sensor>
+      </gazebo>
+    </xacro:if>
+
+  </xacro:macro>
+</robot>

src/simulation/launch/bridge.launch.py

@@ -6,10 +6,10 @@ def generate_launch_description():
         Node(
             package='ros_gz_bridge',
             executable='parameter_bridge',
-            name='depth_camera_bridge',
+            name='zed_point_cloud_bridge',
             output='screen',
             arguments=[
-                '/depth_camera/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked',
+                '/zed/zed_node/point_cloud/cloud_registered@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked',
             ],
         ),
 

src/simulation/launch/spawn.launch.py

@@ -13,7 +13,7 @@
                           description='Start rviz.'),
     DeclareLaunchArgument('rqt', default_value='false',
                           description='Open RQt.'),
-    DeclareLaunchArgument('image_topic', default_value='/depth_camera',
+    DeclareLaunchArgument('image_topic', default_value='/zed/zed_node/left/image_rect_color',
                           description='Topic to start viewing in RQt.'),
     DeclareLaunchArgument('use_sim_time', default_value='true',
                           choices=['true', 'false'],

src/subsystems/navigation/athena_planner/config/nav2_params.yaml

@@ -170,7 +170,7 @@ global_costmap:
         observation_sources: obstacles
         obstacles:
           data_type: "PointCloud2"
-          topic: "/depth_camera/points_corrected"
+          topic: "/zed/zed_node/point_cloud/cloud_registered_corrected"
           marking: true
           clearing: true
           min_obstacle_height: 0.0
@@ -216,7 +216,7 @@ local_costmap:
         observation_sources: obstacles
         obstacles:
           data_type: "PointCloud2"
-          topic: "/depth_camera/points_corrected"
+          topic: "/zed/zed_node/point_cloud/cloud_registered_corrected"
           marking: true
           clearing: true
           min_obstacle_height: 0.0

src/subsystems/navigation/athena_planner/launch/navigation.launch.py

@@ -65,9 +65,9 @@ def generate_launch_description():
         name='point_cloud_filterer',
         parameters=[{
             'use_sim_time': True,
-            'input_topic': '/depth_camera/points',
-            'output_topic': '/depth_camera/points_corrected',
-            'frame_override': 'depth_camera_optical_frame'
+            'input_topic': '/zed/zed_node/point_cloud/cloud_registered',
+            'output_topic': '/zed/zed_node/point_cloud/cloud_registered_corrected',
+            'frame_override': 'zed_left_camera_frame_optical'
         }],
         output='screen',
         emulate_tty=True

src/subsystems/navigation/point_cloud_filterer/src/point_cloud_filtered.cpp

@@ -12,8 +12,8 @@ class PointCloudFilterer : public rclcpp::Node {
   PointCloudFilterer()
   : Node("point_cloud_filterer") {
 
-    input_topic_  = declare_parameter<std::string>("input_topic", "/depth_camera/points");
-    output_topic_ = declare_parameter<std::string>("output_topic", "/depth_camera/points_corrected");
+    input_topic_  = declare_parameter<std::string>("input_topic", "/zed/zed_node/point_cloud/cloud_registered");
+    output_topic_ = declare_parameter<std::string>("output_topic", "/zed/zed_node/point_cloud/cloud_registered_corrected");
 
     frame_override_ = declare_parameter<std::string>("frame_override", "");