Revision history [back]

The robot pose ekf will not start when it only receives messages on this imu's topic, it also expects messages on either the 'vo' or the 'odom' topic. So you need a odometry topic in order to get it working

The robot pose ekf will not start when it only receives messages on this imu's topic, it also expects messages on either the 'vo' or the 'odom' topic. So you need a odometry topic in order to get it working

nav_msgs::Odometry odom;
odomQuaternion = tf::createQuaternionMsgFromYaw(poseAngle);
odom.header.stamp = _currentTime;
odom.header.frame_id = "odom";
odom.pose.pose.position.x = poseX;
odom.pose.pose.position.y = poseY;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odomQuaternion;
odom.pose.covariance[0]  = 0.01;
odom.pose.covariance[7]  = 0.01;
odom.pose.covariance[14] = 99999;
odom.pose.covariance[21] = 99999;
odom.pose.covariance[28] = 99999;
odom.pose.covariance[35] = 0.01;

odom.child_frame_id = "base_footprint";
odom.twist.twist.linear.x = vx;
odom.twist.twist.linear.y = 0;
odom.twist.twist.angular.z = w;
odom.twist.covariance = odom.pose.covariance;

The robot pose ekf will not start when it only receives messages on this imu's topic, it also expects messages on either the 'vo' or the 'odom' topic. So you need a odometry topic in order to get it working

Subscribing to odometry topic publisher, adding covariance to the odometry message then republishing the message may solve your problem in your case.case. Or just edit the source code of the publisher and add some covariance

covariance is added by like this ( it is just an example )

nav_msgs::Odometry odom;
odomQuaternion = tf::createQuaternionMsgFromYaw(poseAngle);
odom.header.stamp = _currentTime;
odom.header.frame_id = "odom";
odom.pose.pose.position.x = poseX;
odom.pose.pose.position.y = poseY;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odomQuaternion;
odom.pose.covariance[0]  = 0.01;
odom.pose.covariance[7]  = 0.01;
odom.pose.covariance[14] = 99999;
odom.pose.covariance[21] = 99999;
odom.pose.covariance[28] = 99999;
odom.pose.covariance[35] = 0.01;

odom.child_frame_id = "base_footprint";
odom.twist.twist.linear.x = vx;
odom.twist.twist.linear.y = 0;
odom.twist.twist.angular.z = w;
odom.twist.covariance = odom.pose.covariance;