ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

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?

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

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

include <ros include<ros ros.h="">

include <tf include<tf transform_broadcaster.h="">

include <nav_msgs include<nav_msgs odometry.h="">

include <sensor_msgs include<sensor_msgs imu.h="">

int main(int argc, char** argv){ argv) {

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

ros::NodeHandle n; n;

ros::Publisher imu_pub = n.advertise<sensor_msgs::imu>("imu", 50); 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; 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();
  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);


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){ argv){

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

ros::NodeHandle n; n;

ros::Publisher odom_pub = n.advertise<nav_msgs::odometry>("odom", 50); 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?

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="">

#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");

"Imu_publisher"); ros::NodeHandle n;

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

n.advertise<sensor_msgs::Imu>("imu", 50); //tf::TransformBroadcaster imu_broadcaster;

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

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

0.1; ros::Time current_time, last_time;

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

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

ros::Time::now(); ros::Rate r(1.0);

while(n.ok())

{

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="">

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

argv){ ros::init(argc, argv, "odometry_publisher");

"odometry_publisher"); ros::NodeHandle n;

n; ros::Publisher odom_pub = n.advertise<nav_msgs::odometry>("odom", 50);

n.advertise<nav_msgs::Odometry>("odom", 50); tf::TransformBroadcaster odom_broadcaster;

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

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

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

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

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?