Revision history [back]

Nitesh_j you were right. THANKS so much!!!!

I have implemented my own cpp code to downsample to 50Hz and set the frequency parameter in robot_localization at 50 Hz as well. I am not sure why it was not working when downsampling with topic_tools. I will add my code in case it is useful for somebody:

#include "ros/ros.h"
#include "sensor_msgs/Imu.h"
#include <iostream>

using namespace std;
class ImuFrequency
{
public:
  ImuFrequency()
  {
  sub_ = nh_.subscribe("imu/data_added_cov", 1, &ImuFrequency::imuCallback, this);
  pub_ = nh_.advertise<sensor_msgs::Imu>("imu/data_low_freq", 1);
}
  void imuCallback(const sensor_msgs::ImuConstPtr &msg);
private:
  ros::NodeHandle nh_;
  ros::Subscriber sub_;
  ros::Publisher pub_;
  sensor_msgs::Imu imu_msg_;
};

void ImuFrequency::imuCallback(const sensor_msgs::ImuConstPtr &msg)
{
  imu_msg_ = *msg;
  ros::Time imu_time = ros::Time::now();
  imu_msg_.header.stamp = imu_time;
  pub_.publish(imu_msg_);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "imu_add_covariances");
ImuFrequency imufrequency;
ros::Rate loop_rate(50);
while(ros::ok())
  {
    ros::spinOnce();
    loop_rate.sleep();
  }
return 0;
}

Here is the angular velocity z from both IMU and odometry. Finally, there is NO DELAY. The odometry covariance does not act weird anymore: