<?xml version="1.0"?>

<robot xmlns:xacro="http://www.ros.org/wiki/xacro">

  <!-- stereo camera macro uses wge100_camera macros -->
  <xacro:include filename="$(find pr2_description)/urdf/sensors/wge100_camera.urdf.xacro" />

  <xacro:include filename="$(find pr2_description)/urdf/sensors/stereo_camera.gazebo.xacro" />

  <xacro:property name="stereo_dx" value="0.00" />
  <xacro:property name="stereo_dy" value="-0.09" /> <!-- +y to the left -->
  <xacro:property name="stereo_dz" value="0.00" />
  <xacro:property name="stereo_rx" value="0.00" />
  <xacro:property name="stereo_ry" value="0.00" />
  <xacro:property name="stereo_rz" value="0.00" />

  <!-- this macro is used for creating wide and narrow double stereo camera links -->
  <xacro:macro name="stereo_camera_v0" params="name parent focal_length hfov image_format image_width image_height *origin">

    <joint name="${name}_frame_joint" type="fixed">
      <xacro:insert_block name="origin" />
      <parent link="${parent}"/>
      <child link="${name}_link"/>
    </joint>
    <!-- camera link is at center of the optical frame, but in x-forward notation -->
    <link name="${name}_link">
      
      <inertial>
        <mass value="0.1" />
        <origin xyz="0 0 0" rpy="0 0 0" />
        <inertia ixx="0.01"  ixy="0"  ixz="0" iyy="0.01"  iyz="0"  izz="0.01" /> <!-- this inertia is made up for now. -->
      </inertial>

    </link>

    <!-- attach optical frame to the camera link -->
    <joint name="${name}_optical_frame_joint" type="fixed">
      <origin xyz="0 0 0" rpy="${-M_PI/2} 0.0 ${-M_PI/2}" /> <!-- rotate frame from x-forward to z-forward camera coords -->
      <parent link="${name}_link"/>
      <child link="${name}_optical_frame"/>
    </joint>
    <!-- optical frame for the stereo camera, with z-forward notation, this is the frame stereo camera images users should refer to -->
    <link name="${name}_optical_frame" type="camera"/>

    <!-- stereo left camera -->
    <xacro:wge100_camera_v0 name="${name}_l_stereo_camera" camera_name="${name}/left" image_format="${image_format}" image_topic_name="image_raw"
                            camera_info_topic_name="camera_info" parent="${name}_link"
                            hfov="${hfov}" focal_length="${focal_length}"
                            frame_id="${name}_optical_frame" hack_baseline="0"
                            image_width="${image_width}" image_height="${image_height}">
      <origin xyz="0 0 0" rpy="0 0 0" />
    </xacro:wge100_camera_v0>

    <!-- stereo right camera -->
    <xacro:wge100_camera_v0 name="${name}_r_stereo_camera" camera_name="${name}/right" image_format="${image_format}" image_topic_name="image_raw"
                            camera_info_topic_name="camera_info" parent="${name}_l_stereo_camera_frame"
                            hfov="${hfov}" focal_length="${focal_length}"
                            frame_id="${name}_optical_frame" hack_baseline="${-stereo_dy}"
                            image_width="${image_width}" image_height="${image_height}">
      <origin xyz="${stereo_dx} ${stereo_dy} ${stereo_dz}" rpy="${stereo_rx} ${stereo_ry} ${stereo_rz}" />
    </xacro:wge100_camera_v0>

    <!-- extensions -->
    <xacro:stereo_camera_gazebo_v0 name="${name}" focal_length="${focal_length}" hfov="${hfov}"
                                   image_width="${image_width}" image_height="${image_height}"/>

  </xacro:macro>
</robot>
