IMU_sensor_controller publishes incomplete covariances

edit

I am trying to use ros_control to publish IMU data, with the eventual goal of combining this with odometry from the robot's encoders in robot_localization. Currently my hardware_interface is largely based on the extremely handy ros_control_boilerplate package. I am using ROS Indigo on Ubuntu 14.04.

The orientation, velocity, and acceleration fields all seem to be published correctly. However, while the hardware_interface is supplying a imu_sensor_interface with full covariances (i.e. each of orientation_, angular_velocity_, and linear_acceleraion_covariance is a 9 element array representing a 3x3 matrix with values along the diagonal), an imu_sensor_controller only publishes the first element of each covariance:

covariance_stored    = [1e-6, 0, 0,  0, 1e-6, 0,  0, 0, 1e-6]
covariance_published = [1e-6, 0, 0,  0, 0,    0,  0, 0, 0   ]

I am setting the covariance fields once, on initializing the hardware_interface, but have confirmed that reading values from the interface in the same way as the imu_sensor_controller shows the complete values:

my_hardware_interface.h defines:

 class MyHardwareInterface: public hardware_interface::RobotHW
 {
 protected:
     ros::NodeHandle                               nh_;

     hardware_interface::ImuSensorInterface       imu_sensor_interface_;

     std::vector<double>                          imu_orientation_cov_;
     std::vector<double>                          imu_angular_vel_cov_;
     std::vector<double>                          imu_linear_acc_cov_;

     //...
 }

my_hardware_interface.cpp initalizes each vector from a parameter:

nh_.getParam("hardware_interface/imu_orientation_covariance",  imu_orientation_cov_);
nh_.getParam("hardware_interface/imu_angular_velocity_covariance", imu_angular_vel_cov_);
nh_.getParam("hardware_interface/imu_linear_acceleration_covariance", imu_linear_acc_cov_);

The following (mimicking imu_sensor_controller::update()) can be run from my_hardware_interface at any point after initialization to print the complete covariances (in the form [a,0,0,0,b,0,0,0,c]):

// converts a pointer to a comma-delimited string
template <typename T> std::string pstr(const T* ptr, int size)
{
    std::stringstream ss;
    if (ptr)
    {
        for (unsigned i=0; i < size; ++i)
        {
            if (i != 0)
            {
                ss << ",";
            }
            ss << ptr[i];
        }
    }
    return ss.str();
}

hardware_interface::ImuSensorHandle imu_sensor = imu_sensor_interface_.getHandle("imu");

ROS_INFO_NAMED("hardware_interface", "orientation covariance: [%s]",
               pstr<double>(imu_sensor.getOrientationCovariance(), 9).c_str());

ROS_INFO_NAMED("babbage_hardware_interface", "angular velocity covariance: [%s]",
               pstr<double>(imu_sensor.getAngularVelocityCovariance(), 9).c_str());

ROS_INFO_NAMED("babbage_hardware_interface", "linear acceleration covariance: [%s]",
               pstr<double>(imu_sensor.getLinearAccelerationCovariance(), 9).c_str());

Does anyone have any insight, or an example project that uses an imu_sensor_interface that I could take a look at? Thanks for your time.