Ask Your Question
0

IMU_sensor_controller publishes incomplete covariances

asked 2015-10-02 10:34:41 -0600

etappan gravatar image

updated 2015-10-02 13:56:14 -0600

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.

edit retag flag offensive close merge delete

1 Answer

Sort by ยป oldest newest most voted
1

answered 2015-10-02 15:04:23 -0600

etappan gravatar image

Looking closer at imu_sensor_controller, the covariance loops (over j, the index of the covariance matrix) all use the index i (the IMU sensor to publish). (Comments are mine.)

// outerloop over the realtime publishers (one per sensor)
for (unsigned i=0; i<realtime_pubs_.size(); i++){
      if (publish_rate_ > 0.0 && last_publish_times_[i] + ros::Duration(1.0/publish_rate_) < time){
        // try to publish
        if (realtime_pubs_[i]->trylock()){
          //...
          // Orientation covariance
          if (sensors_[i].getOrientationCovariance())
          {
            // inner loop over the indices of the covariance
            for (unsigned j=0; j<realtime_pubs_[i]->msg_.orientation_covariance.size(); ++j){
              // all of these are being access by 'i', none are being access by 'j'
              realtime_pubs_[i]->msg_.orientation_covariance[i] = sensors_[i].getOrientationCovariance()[i];
            }
          }
edit flag offensive delete link more

Comments

Good assessment. Could you please make a pull request with the fix against the indigo-devel branch of ros_controllers?.

Adolfo Rodriguez T gravatar imageAdolfo Rodriguez T ( 2015-10-05 03:00:00 -0600 )edit

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

1 follower

Stats

Asked: 2015-10-02 10:34:41 -0600

Seen: 252 times

Last updated: Oct 02 '15