5 sec delay in yaw rate fusing radar and IMU Robot Localization [closed]

asked 2020-05-06 04:17:00 -0500

xaru8145
105 ●8 ●13 ●17

updated 2022-01-22 16:10:32 -0500

Evgeny
26 ●6

I am using a ground robot and trying to fuse x velocity from the radar and angular z velocity from the IMU in Robot Localization to estimate odometry. I have a consistent 5 sec delay between the angular rate of the IMU vs the angular rate of the odometry message. This obviously make my robot do the turns 5 seconds later than it should.

Bagfile

IMU message

header: 
  seq: 6420
  stamp: 
    secs: 1583884429
    nsecs:   8326609
  frame_id: "imu_link"
orientation: 
  x: -0.00182108255529
  y: -0.0154610365419
  z: -0.592995859678
  w: 0.805054998377
orientation_covariance: [1e-05, 0.0, 0.0, 0.0, 1e-05, 0.0, 0.0, 0.0, 1e-05]
angular_velocity: 
  x: -0.0146767685242
  y: -0.00264796876277
  z: 0.00796559285187
angular_velocity_covariance: [1.218467815533586e-07, 0.0, 0.0, 0.0, 1.218467815533586e-07, 0.0, 0.0, 0.0, 1.218467815533586e-07]
linear_acceleration: 
  x: 0.262929823748
  y: 0.0700912966914
  z: 9.64307111704
linear_acceleration_covariance: [8.661248102725949e-06, 0.0, 0.0, 0.0, 8.661248102725949e-06, 0.0, 0.0, 0.0, 8.661248102725949e-06]

Radar velocity message

header: 
  seq: 908
  stamp: 
    secs: 1583884414
    nsecs: 817418000
  frame_id: "base_radar_link"
twist: 
  twist: 
    linear: 
      x: 0.955666473669
      y: 0.0772511278807
      z: -0.179226648605
    angular: 
      x: 0.0
      y: 0.0
      z: 0.0
  covariance: [0.01, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.015, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.05, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]

Params

frequency: 30
sensor_timeout: 0.1
two_d_mode: true
transform_time_offset: 0.0
transform_timeout: 0.0
print_diagnostics: true
debug: false
debug_out_file: /path/to/debug/file.txt
publish_tf: true
publish_acceleration: false

map_frame: map              # Defaults to "map" if unspecified
odom_frame: odom            # Defaults to "odom" if unspecified
base_link_frame: base_link  # Defaults to "base_link" if unspecified
world_frame: odom           # Defaults to the value of odom_frame if unspecified

twist0: /mmWaveDataHdl/velocity
twist0_config: [false, false, false,
                false, false, false,
                true,  false, false,
                false, false, false,
                false, false, false]
twist0_queue_size: 3
twist0_nodelay: false

# Jackal's IMU is in ENU frame so it conforms to REP-103
imu0: /imu/data_added_cov
imu0_config: [false, false, false,
              false, false, false,
              false, false, false,
              false,false, true,
              false, false, false]
imu0_nodelay: false
imu0_differential: false
imu0_relative: true
imu0_queue_size: 5

My robot cannot move laterally and it is in 2D mode that is why only the x component of the linear velocity is fused in. Here is a plot in Matlab of a part of the bagfile. You can see how the radar velocity vs odom velocity are both synchronized. However, the angular rate in the odometry is 5 seconds delayed compared to its respective IMU rate (which should be the same since it is the only input for angular rate). image description

There is something a bit weird with the covariance. It starts ramping up until 5 seconds and then it goes back down again. The value of ... (more)

edit retag flag offensive reopen merge delete

Closed for the following reason the question is answered, right answer was accepted by xaru8145
close date 2020-05-06 22:24:06.504716

add a comment

2 Answers

Sort by » oldest newest most voted

answered 2020-05-06 12:10:06 -0500

Nitesh_j
65 ●17 ●4

I have been on the same situation...its memory leak it's pretty common , set the IMU publisher and EKF freq. to 50 it will sync. never publish any publishers at 70Hz, always go with [10,20,50,80,100,120,150]Hz, these are values worked for me try it out and let me know

edit flag offensive delete link

Comments

Thanks for you answer! I have tried what you suggested but did not work. I have use the topic_tools to set the IMU rate to 50Hz. However, I was not getting more than 42 Hz so I tried with 20 Hz for both the robot_localization and the IMU (did not change anything).

  <node name="foo_throttler" type="throttle" pkg="topic_tools" args="messages /imu/data_added_cov 20 /imu/data_low_freq" />

xaru8145 ( 2020-05-06 13:41:13 -0500 )edit

How Ur publishing the imu sensor values are using python or c++, which software are using for benchmarking the o/p Freq.

Have u tried rqt to dynamic reconfigure the parameters?

Nitesh_j ( 2020-05-06 13:48:00 -0500 )edit

I have the Jackal ground robot from Clearpath that is publishing the imu/data_raw Imu topic. I use the imu_filter_madgwick to filter the data to imu/data. This publishes empty covariance values in the orientation so I made a simple cpp code to add a small value to those (code in the next comment). This node publishes imu/data_added_cov at 70 Hz and is the one I recorded in the bag file. I am not sure I do not think I can reconfigure parameters since I am replaying from a bagfile. And to lower the frequency I just run the topic_tools package. But maybe this is not the way I need to write another cpp node using timer or rate.

xaru8145 ( 2020-05-06 14:01:21 -0500 )edit

Code:

#include "ros/ros.h"
#include "sensor_msgs/Imu.h"

ros::Publisher imu_pub;
void imuCallback(const sensor_msgs::Imu& msg)
{
  sensor_msgs::Imu imu_msg;
  imu_msg = msg;
  imu_msg.orientation_covariance[0] = 0.00001;
  imu_msg.orientation_covariance[4] = 0.00001;
  imu_msg.orientation_covariance[8] = 0.00001;

  ros::Time imu_time = ros::Time::now();
  imu_msg.header.stamp = imu_time;
  imu_pub.publish(imu_msg);
}

int main(int argc, char **argv)
{

ros::init(argc, argv, "imu_add_covariances");
ros::NodeHandle n;
ros::Subscriber imu_sub = n.subscribe("imu/data", 1000, imuCallback);
imu_pub = n.advertise<sensor_msgs::Imu>("imu/data_added_cov", 1000);
ros::spin();

return 0;
}

xaru8145 ( 2020-05-06 14:01:40 -0500 )edit

add a comment

answered 2020-05-06 21:13:14 -0500

xaru8145
105 ●8 ●13 ●17

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:

image description

edit flag offensive delete link

Comments

Nice all the best dude ,pls close this issue

Nitesh_j ( 2020-05-06 22:19:36 -0500 )edit

Wow this is really something new. Most IMU ROS drivers I know publish the IMU message at 100-400Hz. Does this mean that all these IMU messages must be downsampled to 50Hz for robot localization fusion to work?

hashirzahir ( 2020-05-07 05:47:07 -0500 )edit

For my configuration and the sensors I am using in robot localization, downsampling to 50 Hz solved my issue. If RL works for you straightaway you should not worry about it. But If you have the same issue as me, you might consider Nitesh_j suggestion of going with these frequencies : [10,20,50,80,100,120,150] Hz

xaru8145 ( 2020-05-15 17:13:04 -0500 )edit

add a comment

5 sec delay in yaw rate fusing radar and IMU Robot Localization [closed]

Closed for the following reason the question is answered, right answer was accepted by xaru8145
close date 2020-05-06 22:24:06.504716

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

5 sec delay in yaw rate fusing radar and IMU Robot Localization [closed] edit

Closed for the following reason the question is answered, right answer was accepted by xaru8145 close date 2020-05-06 22:24:06.504716

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

5 sec delay in yaw rate fusing radar and IMU Robot Localization [closed]

Closed for the following reason the question is answered, right answer was accepted by xaru8145
close date 2020-05-06 22:24:06.504716