stereo_generic_camera

<?xml version="1.0"?>
<!-- XML namespaces -->
<robot xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
       xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
       xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
       xmlns:xacro="http://ros.org/wiki/xacro">

    <xacro:macro name="stereo_camera_generic" params="parent name camera_name image_format image_topic_name
                                                        camera_info_topic_name parent hfov focal_length
                                                        hack_baseline image_width image_height
                                                        update_rate stereo_dx stereo_dy stereo_dz
                                                        stereo_rx stereo_ry stereo_rz">

        <joint name="${name}_left_frame_joint" type="fixed">
            <origin xyz="${stereo_dx} ${stereo_dy} ${stereo_dz}" rpy="${stereo_rx} ${stereo_ry} ${stereo_rz}" />
            <parent link="${parent}"/>
            <child link="${name}_left_frame"/>
        </joint>

        <link name="${name}_left_frame">
            <inertial>
                <mass value="0.01" />
                <origin xyz="0 0 0" />
                <inertia ixx="0.001"  ixy="0.0"  ixz="0.0"
                        iyy="0.001"  iyz="0.0"
                        izz="0.001" />
            </inertial>
        </link>

        <joint name="${name}_right_frame_joint" type="fixed">
          <origin xyz="{-stereo_dx} {-stereo_dy} {-stereo_dz}" rpy="${stereo_rx} ${stereo_ry} ${stereo_rz}" />
          <parent link="${parent}"/>
          <child link="${name}_right_frame"/>
        </joint>

        <link name="${name}_right_frame">

          <inertial>
            <mass value="0.01" />
            <origin xyz="0 0 0" rpy="0 0 0"/>
            <inertia ixx="0.001"  ixy="0.0"  ixz="0.0"
                     iyy="0.001"  iyz="0.0"
                     izz="0.001" />
          </inertial>

        </link>

        <link name="${name}_optical_frame" type="camera"/>

        <joint name="${name}_optical_frame_joint_left" type="fixed">
          <origin xyz="0 0 0" rpy="${-M_PI/2} 0.0 ${-M_PI/2}" />
          <parent link="${name}_left_frame" />
          <child link="${name}_optical_frame"/>
        </joint>

        <joint name="${name}_optical_frame_joint_right" type="fixed">
          <origin xyz="0 0 0" rpy="${-M_PI/2} 0.0 ${-M_PI/2}" />
          <parent link="${name}_right_frame" />
          <child link="${name}_optical_frame"/>
        </joint>

        <gazebo reference="${name}_left_frame">
        <sensor:camera name="${name}_left_sensor">
            <imageSize>${image_width} ${image_height}</imageSize>
            <imageFormat>${image_format}</imageFormat>
            <hfov>${hfov}</hfov>
            <nearClip>0.1</nearClip>
            <farClip>100</farClip>
            <updateRate>${update_rate}</updateRate>
            <controller:gazebo_ros_camera name="${name}_left_controller" plugin="libgazebo_ros_camera.so">
              <alwaysOn>true</alwaysOn>
              <updateRate>${update_rate}</updateRate>
              <cameraName>${camera_name}/left</cameraName>
              <imageTopicName>${image_topic_name}</imageTopicName>
              <cameraInfoTopicName>${camera_info}</cameraInfoTopicName>
              <frameName>${name}_optical_frame</frameName>
              <hackBaseline>${hack_baseline}</hackBaseline>
              <CxPrime>${(image_width+1)/2}</CxPrime>
              <Cx>${(image_width+1)/2}</Cx>
              <Cy>${(image_height+1)/2}</Cy>
              <focal_length>${focal_length}</focal_length>
              <distortion_k1>0.0</distortion_k1>
              <distortion_k2>0.0</distortion_k2>
              <distortion_k3>0.0</distortion_k3>
              <distortion_t1>0.0</distortion_t1>
              <distortion_t2>0.0</distortion_t2>
              <interface:camera name="${name}_stereo_left_iface" />
            </controller:gazebo_ros_camera>
          </sensor:camera>
            <turnGravityOff>true</turnGravityOff>
            <material>PR2/Blue</material>
        </gazebo>

        <gazebo reference="${name}_right_frame">
        <sensor:camera name="${name}_right_sensor">
            <imageSize>${image_width} ${image_height}</imageSize>
            <imageFormat>${image_format}</imageFormat>
            <hfov>${hfov}</hfov>
            <nearClip>0.1</nearClip>
            <farClip>100</farClip>
            <updateRate>${update_rate}</updateRate>
            <controller:gazebo_ros_camera name="${name}_right_controller" plugin="libgazebo_ros_camera.so">
              <alwaysOn>true</alwaysOn>
              <updateRate>${update_rate}</updateRate>
              <cameraName>${camera_name}/right</cameraName>
              <imageTopicName>${image_topic_name}</imageTopicName>
              <cameraInfoTopicName>${camera_info}</cameraInfoTopicName>
              <frameName>${name}_optical_frame</frameName>
              <hackBaseline>${hack_baseline}</hackBaseline>
              <CxPrime>${(image_width+1)/2}</CxPrime>
              <Cx>${(image_width+1)/2}</Cx>
              <Cy>${(image_height+1)/2}</Cy>
              <focal_length>${focal_length}</focal_length>
              <distortion_k1>0.0</distortion_k1>
              <distortion_k2>0.0</distortion_k2>
              <distortion_k3>0.0</distortion_k3>
              <distortion_t1>0.0</distortion_t1>
              <distortion_t2>0.0</distortion_t2>
              <interface:camera name="${name}_stereo_right_iface" />
            </controller:gazebo_ros_camera>
          </sensor:camera>
            <turnGravityOff>true</turnGravityOff>
            <material>PR2/Blue</material>
        </gazebo>

  </xacro:macro>

</robot>