Hector localization error [closed]

edit

Hello

I tried the Hector Localization package but I got some errors. I can compile without errors. But to get work I had to create a node that split the two geometry massages namely the PoseWithCovarianceStamped and TwistWithCovarianceStamped. Here is my node

#include "ros/ros.h"
#include "geometry_msgs/PoseWithCovarianceStamped.h"
#include "geometry_msgs/TwistWithCovarianceStamped.h"
#include "nav_msgs/Odometry.h"
#include "tf/transform_datatypes.h"

ros::Publisher p_pub,v_pub;
geometry_msgs::PoseWithCovarianceStamped p_out;
geometry_msgs::TwistWithCovarianceStamped t_out;

double dt, last_x,last_y,last_t, last_yaw;
bool is_first = 1;

double normalize(double in)
{
  if(in < -M_PI) return in + 2*M_PI;
  else if(in > M_PI) return in - 2*M_PI;
  else return in;
}

void oCallback(const nav_msgs::Odometry::ConstPtr& msg)
{
  if(!is_first)
  {
    dt = msg->header.stamp.toSec() - last_t;
    t_out.twist.twist.linear.x = (msg->pose.pose.position.x - last_x) /dt;
    t_out.twist.twist.linear.y = (msg->pose.pose.position.y - last_y) /dt;
    t_out.twist.twist.linear.z = 0;
    t_out.twist.twist.angular.x=t_out.twist.twist.angular.y = 0;
    t_out.twist.twist.angular.z = normalize(tf::getYaw(msg->pose.pose.orientation)-last_yaw) /dt;

    t_out.twist.covariance = msg->pose.covariance;

    p_out.pose = msg->pose;
    p_out.header = t_out.header = msg->header;
    p_pub.publish(p_out); v_pub.publish(t_out);
  }
  else is_first = 0;

  last_t = msg->header.stamp.toSec();
  last_x = msg->pose.pose.position.x;
  last_y = msg->pose.pose.position.y;
  last_yaw = tf::getYaw(msg->pose.pose.orientation);
}
int main(int argc, char **argv)
{
    ros::init(argc, argv, "odom_splitting_node");
    ros::NodeHandle n;

    p_pub = n.advertise<geometry_msgs::PoseWithCovarianceStamped>("poseupdate", 1);
    v_pub = n.advertise<geometry_msgs::TwistWithCovarianceStamped>("twistupdate", 1);
    ros::Subscriber sub = n.subscribe("/odom", 1, oCallback);

    ros::spin();

    return 0;
}

Im using IMU and odometry from the encoders. But Im getting this error when Im running it.

Ignoring poseupdate for twist as the a-priori state covariance is zero!
[ WARN] [1360729243.709860468, 1357879676.617191090]: Update for position_z with error [ -0.213894 ], |error| = 144085 sigma exceeds max_error!
[ WARN] [1360729243.709996003, 1357879676.617191090]: Ignoring poseupdate for twist as the a-priori state covariance is zero!
[ WARN] [1360729243.710123133, 1357879676.617191090]: Update for position_z with error [ -0.450039 ], |error| = 305886 sigma exceeds max_error!
[ WARN] [1360729243.710234269, 1357879676.617191090]: Ignoring poseupdate for twist as the a-priori state covariance is zero

Any help?? !