How to solve "Timestamps of odometry and imu are x seconds apart"?
I am getting the "Timestamps of odometry and imu are x seconds apart" problem listed in robot_pose_ekf troubleshooting. What are the possible ways to solve this?
I am currently publishing to /odom and /imu/data in two separate nodes. I notice that turtlebot publishes both in the same turtlebot_node. Is this the only way to get the two timestamps to sync?
EDIT (code snippets of publishing nodes, launch file uses the same robot_pose_ekf parameters found in turtlebot's minimal.launch)
for node publishing odometry:
odom_pub = nh.advertise<nav_msgs::Odometry>("odom", 1);
...
void publish_odom(double x, double y, double theta)
{
// store the current pose
prevPose.x = cur_x;
prevPose.y = cur_y;
prevPose.theta = cur_theta;
// calculate the new pose
trobot.calcOdom(&cur_x,&cur_y,&cur_theta);
current_time = ros::Time::now();
double dt = (current_time - last_time).toSec();
// convert rotation about z into quaternion
geometry_msgs::Quaternion odom_quat;
odom_quat.z=sin(cur_theta/2.0);
odom_quat.w=cos(cur_theta/2.0);
//next, we'll publish the odometry message over ROS
nav_msgs::Odometry odom;
odom.header.stamp = current_time;
odom.header.frame_id = "odom";
odom.child_frame_id = "base_footprint";
//set the position
odom.pose.pose.position.x = cur_x;
odom.pose.pose.position.y = cur_y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;
odom.pose.covariance =
boost::array<double, 36>{{1e-3, 0, 0, 0, 0, 0,
0, 1e-3, 0, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e-3}};
//set the velocity
odom.twist.twist.linear.x = (cur_x-prevPose.x)/dt;
odom.twist.twist.linear.y = (cur_y-prevPose.y)/dt;
odom.twist.twist.angular.z = (cur_theta-prevPose.theta)/dt;
odom.twist.covariance =
boost::array<double, 36>{{1e-3, 0, 0, 0, 0, 0,
0, 1e-3, 0, 0, 0, 0,
0, 0, 1e6, 0, 0, 0,
0, 0, 0, 1e6, 0, 0,
0, 0, 0, 0, 1e6, 0,
0, 0, 0, 0, 0, 1e-3}};
//publish the message
odom_pub.publish(odom);
last_time = current_time;
}
And for the imu (covariance values defined in initialisation):
imu_data_pub_ = imu_node_handle.advertise<sensor_msgs::Imu>("data", 100);
...
void publish_imu_data()
{
// get gyro-stabilized quaternion, instantaneous acceleration and bias-compensated angular rate
if(imu.ReadQuatVect(comPort, quaternion, accel, angRate))
{
// convert North-East-Down (NED) to East-North-Up (ENU) coordinates
imu_data.orientation.w = quaternion.w;
imu_data.orientation.x = quaternion.y;
imu_data.orientation.y = quaternion.x;
imu_data.orientation.z = -quaternion.z;
imu_data.linear_acceleration.x = accel.y;
imu_data.linear_acceleration.y = accel.x;
imu_data.linear_acceleration.z = -accel.z;
imu_data.angular_velocity.x = angRate.y;
imu_data.angular_velocity.y = angRate.x;
imu_data.angular_velocity.z = -angRate.z;
if(errcnt>0) errcnt--;
imu_data.header.stamp = ros::Time::now();
imu_data.header.frame_id = "gyro_link";
imu_data_pub_.publish(imu_data);
}
else errcnt++;
// removed: code to handle large errcnt
}