Robot_pose_ekf does not display the pose in rviz [closed]

edit

Hello

Im using robot_pose_ekf package and have some problems with publishing and displaying the pose. My sensor package consists of IMU and laser. So because of that I made own node that publish and odometry topic. So it subscribe to odometry topic publisher, add covariance to the odometry message then republished the message. This is the node I create

double time_last, xlast = 0, ylast = 0, yaw_last = 0, delta_t, curr_yaw;

void initialize(void)
{
  odom.header.frame_id = "map";
  odom.child_frame_id = "base_footprint";
  odom.pose.pose.position.z = 0;
}

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 poseCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
{
  odom.pose.pose.position.x = msg->pose.pose.position.x;
  odom.pose.pose.position.y = msg->pose.pose.position.y;
  odom.pose.pose.orientation = msg->pose.pose.orientation;
  odom.header.stamp = msg->header.stamp;

//Speed calculation
delta_t = ros::Time::now().toSec() - time_last;
odom.twist.twist.linear.x = (msg->pose.pose.position.x-xlast)/delta_t;
odom.twist.twist.linear.y = (msg->pose.pose.position.y-ylast)/delta_t;
curr_yaw = tf::getYaw(msg->pose.pose.orientation);
odom.twist.twist.angular.z = normalize(curr_yaw-yaw_last)/delta_t;

  pub.publish(odom);
xlast = msg->pose.pose.position.x; ylast = msg->pose.pose.position.y; yaw_last = curr_yaw;
  time_last = ros::Time::now().toSec();
}

void setup()
{
  odom.twist.twist.linear.z = odom.twist.twist.angular.x = odom.twist.twist.angular.y = 0;
time_last = ros::Time::now().toSec();

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.twist.covariance=odom.pose.covariance;// = msg->pose.covariance;
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "converter");
  initialize();
  ros::NodeHandle nh;

setup();
  ros::Subscriber sub = nh.subscribe("poseupdate", 1, poseCallback);
  pub = nh.advertise<nav_msgs::Odometry>("wheelodom", 1);

  ros::spin();
}

This is my launch file

<launch>
    <param name="/use_sim_time" value="true"/>

    <param name="use_sim_time" value="true"/>
    <node name="rosplay" pkg="rosbag" type="play" args="/home/Test-13-09-45.bag --clock "/>
    <node name="hector_mapping" pkg="hector_mapping" type="hector_mapping" output="screen">
        <param name="pub_map_odom_transform" value="true"/>
    <param name="map_frame" value="map"/>
        <param name="base_frame" value="scanmatcher_frame"/>
        <param name="odom_frame" value="scanmatcher_frame"/>

    </node>

    <node pkg="tf" type="static_transform_publisher" name="baselink_laser" args="0 0 0 0 0 0 /scanmatcher_frame /laser 10"/>
    <node pkg="tf" type="static_transform_publisher" name="corner_a" args="0.45 0.3 0 0 0 0 /scanmatcher_frame /corner1 10"/>
    <node pkg="tf" type="static_transform_publisher" name="corner_b" args="0.45 -0.3 0 0 0 0 /scanmatcher_frame /corner2 10"/>
    <node pkg="tf" type="static_transform_publisher" name="corner_c" args="-0.45 -0.3 0 0 0 0 /scanmatcher_frame /corner3 10"/>
    <node pkg="tf" type="static_transform_publisher" name="corner_d" args="-0.45 0.3 0 0 0 0 /scanmatcher_frame /corner4 10"/>


    <!-- Start the map server node and specify the map file (*.pgm) and the map resolution in metres/pixel -->
    <node name="map_server" pkg="map_server" type="map_server" args="$(find amcl_listener)/maps/pow_real_time.yaml"         output="screen"/>


     EKF to fuse IMU ...