Simulating a rotated Phidgets IMU

edit

Summary: I have a Phidgets IMU, using the Madgwick filter, mounted rotated in 2 dimensions on the robot frame. I use the orientation from /imu/data and knowledge of the rotated mount to produce a good transform from odom => base_link. This all works fine. However, I'm struggling to figure out how to use one of the built-in IMU simulations in Gazebo to simulate this setup. There's an "rpyoffset" field, but the multiplication order looks wrong to me. Has anybody had luck simulating rotated IMUs in Gazebo?

Thanks in advance. Details below:

URDF:

  <!-- IMU -->
  <link name="imu_link">

    <inertial>
      <origin rpy="${-PI/2} ${-PI/2} 0" xyz="0 0 0"/>
      <mass value="0"/>
      <inertia
        ixx="1.0" ixy="0.0" ixz="0.0"
        iyy="1.0" iyz="0.0"
        izz="1.0"/>
    </inertial>

    <visual>
      <origin rpy="${-PI/2} ${-PI/2} 0" xyz="0 0 0"/> <!-- rotated -->
      <geometry>
        <box size="0.05 0.03 0.005"/>
      </geometry>
      <material name="green">
        <color rgba="0 0.5 0 0.5"/>
      </material>
    </visual>

    <collision>
      <origin rpy="${-PI/2} ${-PI/2} 0" xyz="0 0 0"/>
      <geometry>
        <box size="0.05 0.03 0.005"/>
      </geometry>
    </collision>

  </link>

  <!-- Attach imu link to base link -->
  <joint name="base_link_to_imu_link" type="fixed">
    <parent link="base_link"/>
    <child link="imu_link"/>
    <origin rpy="0 0 0" xyz="0.1 -0.025 0.06"/>
  </joint>

  <gazebo reference="imu_link">
    <gravity>true</gravity>
    <sensor name="imu_sensor" type="imu">
      <update_rate>10</update_rate>
      <plugin name="imu_plugin" filename="libgazebo_ros_imu_sensor.so">
        <robotNamespace></robotNamespace>
        <topicName>imu/data</topicName>
        <bodyName>imu_link</bodyName>
        <updateRateHZ>10.0</updateRateHZ>
        <gaussianNoise>0.0</gaussianNoise>
        <xyzOffset>0 0 0</xyzOffset>
        <rpyOffset>0 0 0</rpyOffset> <!-- This doesn't do what I expect -->
        <frameName>imu_link</frameName>
      </plugin>
      <pose>0 0 0 0 0 0</pose>
    </sensor>
  </gazebo>

Robot setup:

  nh_priv_.param("simulation", simulation_, true);
  if (simulation_)
  {
    // Hack -- can't simulate a rotated IMU
    imu_transform_ = tf2::Quaternion::getIdentity();
  }
  else
  {
    tf2::Quaternion imu_orientation;
    imu_orientation.setRPY(-M_PI/2, -M_PI/2, 0);
    imu_transform_ =  imu_orientation.inverse();
  }

Robot /imu/data callback:

void Robot::imuCallback(const sensor_msgs::ImuConstPtr &msg)
{
  tf2::Quaternion imu_orientation;
  tf2::fromMsg(msg->orientation, imu_orientation);
  base_orientation_ = imu_orientation * imu_transform_;
}

Odom publisher:

void Robot::publishOdom()
{
  geometry_msgs::TransformStamped odom_tf;
  odom_tf.header.stamp = ros::Time::now();
  odom_tf.header.frame_id = "odom";
  odom_tf.child_frame_id = "base_link";
  odom_tf.transform.translation.x = 0; // TODO
  odom_tf.transform.translation.y = 0; // TODO
  odom_tf.transform.translation.z = 0; // TODO
  odom_tf.transform.rotation = tf2::toMsg(base_orientation_);
  tf_broadcaster_.sendTransform(odom_tf);
}

Here's the code from Gazebo ROS IMU Sensor -- note the multiplication order (offset * reading, not reading * offset).