Simulated 3-axis camera gimbal for UAV in gazebo

edit

Hi there,

I'm currently trying to simulate a "perfect" (no friction) 3-axis camera gimbal attached on the bottom of a UAV. I'm using the hector quadrotor gazebo/ROS project to try and achieve this.

The ultimate goal is that the simulated image plane is continuously perpendicular with the world plane.

In order to achieve this, I've attached a simulated camera to 3 orthogonal, continuous joints via links with no length (see the URDF/xacro below). However, the image feed produced is still as if the camera were permanently affixed to it.

I'm fairly new to URDF/xacro/etc. so please excuse me if this is just a basic misunderstanding on my part!

Here's the gimbal URDF/xacro file I've created:

<?xml version="1.0"?>

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

  <xacro:property name="M_PI" value="3.1415926535897931" />

  <xacro:macro name="gimbal_camera" params="name parent *origin ros_topic cam_info_topic update_rate res_x res_y image_format hfov">

    <!-- Gimbal attached to UAV -->
    <joint name="gimbal_joint" type="fixed">
        <xacro:insert_block name="origin" />
        <parent link="${parent}" />
        <child link="gimbal_r_link" />
    </joint>

    <link name="gimbal_r_link">
      <inertial>
        <mass value="0" />
        <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
      </inertial>
    </link>

    <joint name="gimbal_r_joint" type="continuous">
        <parent link="gimbal_r_link" />
        <child link="gimbal_p_link" />
        <axis xyz="1 0 0" />
        <dynamics damping="0.0" friction="0.0" />
    </joint>

    <link name="gimbal_p_link">
      <inertial>
        <mass value="0" />
        <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
      </inertial>
    </link>

    <joint name="gimbal_p_joint" type="continuous">
        <parent link="gimbal_p_link" />
        <child link="gimbal_y_link" />
        <axis xyz="0 1 0" />
        <dynamics damping="0.0" friction="0.0" />
    </joint>

    <link name="gimbal_y_link">
      <inertial>
        <mass value="0" />
        <inertia ixx="0" ixy="0" ixz="0" iyy="0" iyz="0" izz="0" />
      </inertial>
    </link>

    <joint name="gimbal_y_joint" type="continuous">
        <parent link="gimbal_y_link" />
        <child link="${name}_link" />
        <axis xyz="0 0 1" />
        <dynamics damping="0.0" friction="0.0" />
    </joint>

    <link name="${name}_link">
    </link>

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

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

    <gazebo reference="gimbal_r_link">
      <sensor type="camera" name="${name}_camera_sensor">
        <update_rate>${update_rate}</update_rate>
        <camera>
          <horizontal_fov>${hfov * M_PI/180.0}</horizontal_fov>
          <image>
            <format>${image_format}</format>
            <width>${res_x}</width>
            <height>${res_y}</height>
          </image>
          <clip>
            <near>0.01</near>
            <far>100</far>
          </clip>
        </camera>

        <plugin name="${name}_camera_controller" filename="libgazebo_ros_camera.so">
          <cameraName>${name}</cameraName>
          <imageTopicName>${ros_topic}</imageTopicName>
          <cameraInfoTopicName>${cam_info_topic}</cameraInfoTopicName>
          <frameName>${name}_optical_frame</frameName>
        </plugin>
      </sensor>
    </gazebo>
  </xacro:macro>

</robot>

Which is instantiated in the following file:

<?xml version="1.0"?>

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

    <xacro:property name="M_PI" value="3.1415926535897931" />

    <!-- Included URDF Files -->
    <xacro:include filename="$(find hector_quadrotor_description)/urdf/quadrotor_base.urdf.xacro" />
    <xacro:include filename="$(find hector_sensors_description)/urdf/gimbal_camera.urdf.xacro" />

    <!-- Instantiate quadrotor_base_macro once (has no parameters atm) -->
    <xacro:quadrotor_base_macro />

    <!-- Downward facing camera attached to gimbal -->
    <xacro:gimbal_camera name="downward_cam" parent="base_link" ros_topic="camera/image" cam_info_topic="camera/camera_info ...