Computing Robot_position of robot using Robot_localization

edit

Tom and Anas, Thanx, I tried and wrote 2 different files,

Imu.cpp for publishing imu data, (hard coded) for testing

#include<ros/ros.h> 
#include<tf/transform_broadcaster.h>
#include<nav_msgs/Odometry.h>
#include<sensor_msgs/Imu.h>
int main(int argc, char** argv)
{

    ros::init(argc, argv, "Imu_publisher");

  ros::NodeHandle n;

 ros::Publisher imu_pub = n.advertise<sensor_msgs::Imu>("imu", 50);

 //tf::TransformBroadcaster imu_broadcaster;

  double x = 0.0;
  double y = 0.0;
  double th = 0.0;

  double vx = 0.1;
 double vy = -0.1;
 double vth = 0.1;

  ros::Time current_time, last_time;

  current_time = ros::Time::now();

  last_time = ros::Time::now();

  ros::Rate r(1.0);

  while(n.ok())

 {

     current_time = ros::Time::now();

     //next, we'll publish the IMU message over ROS

     sensor_msgs::Imu fl;

        // fl.header.seq=_simulationFrameID;

           fl.header.stamp=current_time;

           fl.orientation.x=0.0; //TODO orientation

           fl.orientation.y=0.0;

           fl.orientation.z=0.0;

           fl.orientation.w=1.0;

           fl.angular_velocity.x=1.3;

           fl.angular_velocity.y=1.2;

           fl.angular_velocity.z=1.5;

           fl.linear_acceleration.x=1.3;

           fl.linear_acceleration.y=1.6;

           fl.linear_acceleration.z=1.4;

          //publish the message

         imu_pub.publish(fl);

          last_time = current_time;

            r.sleep();

       }

      }

and Odom.cpp for publishing /odom data,

  #include <ros/ros.h>
  #include <tf/transform_broadcaster.h>
  #include <nav_msgs/Odometry.h>
  int main(int argc, char** argv){
  ros::init(argc, argv, "odometry_publisher");
  ros::NodeHandle n;
  ros::Publisher odom_pub = n.advertise<nav_msgs::Odometry>("odom", 50);
  tf::TransformBroadcaster odom_broadcaster;
  double x = 0.0;
  double y = 0.0;
  double th = 0.0;

  double vx = 0.1;
  double vy = -0.1;
  double vth = 0.1;

   ros::Time current_time, last_time;
   current_time = ros::Time::now();
   last_time = ros::Time::now();

    ros::Rate r(1.0);
    while(n.ok()){
    current_time = ros::Time::now();

   //compute odometry in a typical way given the velocities of the robot
     double dt = (current_time - last_time).toSec();
       double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
       double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
      double delta_th = vth * dt;

      x += delta_x;
       y += delta_y;
     th += delta_th;

//since all odometry is 6DOF we'll need a quaternion created from yaw
   geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(th);

//first, we'll publish the transform over tf
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";

odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;

//send the transform
odom_broadcaster.sendTransform(odom_trans);

//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_link";

//set the position
  odom.pose.pose.position.x = x;
  odom.pose.pose.position.y = y;
  odom.pose.pose.position.z = 0.0;
  odom.pose.pose.orientation = odom_quat;

//set the velocity
  odom.twist.twist.linear.x = vx;
  odom.twist.twist.linear.y = vy;
  odom.twist.twist.angular.z = vth;

   //publish the message
  odom_pub.publish(odom);

      last_time = current_time;
   r.sleep();
   }
    }

is it correct? I am getting covariance matrix, 0.0 for all, why? is there any error?