Go to the documentation of this file.00001 #include<ros/ros.h>
00002 #include<tf/transform_broadcaster.h>
00003 #include <tf/transform_listener.h>
00004 #include<nav_msgs/Odometry.h>
00005
00006 tf::Quaternion base2odom_quad(0,0,0,1);
00007 tf::Vector3 base2odom_pos(0,0,0);
00008 tf::TransformBroadcaster *broadcaster;
00009 tf::TransformListener *listener;
00010 void groundTruePosCallback(const nav_msgs::Odometry::ConstPtr& msg){
00011 float x=msg->pose.pose.orientation.x;
00012 float y=msg->pose.pose.orientation.y;
00013 float z=msg->pose.pose.orientation.z;
00014 float w=msg->pose.pose.orientation.w;
00015 float pos_x=msg->pose.pose.position.x;
00016 float pos_y=msg->pose.pose.position.y;
00017 float pos_z=msg->pose.pose.position.z;
00018 ros::Time current_time=msg->header.stamp;
00019 base2odom_quad=tf::Quaternion(x,y,z,w);
00020 base2odom_pos=tf::Vector3(pos_x,pos_y,pos_z);
00021 tf::Transform base2odom=tf::Transform(base2odom_quad,base2odom_pos);
00022 broadcaster->sendTransform(
00023 tf::StampedTransform(
00024 base2odom,current_time,"odom","base_footprint"));
00025 }
00026
00027 int main(int argc, char** argv){
00028 ros::init(argc,argv,"odom_tf_publisher");
00029 ros::NodeHandle n;
00030 ros::Subscriber sub=n.subscribe("base_pose_ground_truth",100,groundTruePosCallback);
00031 broadcaster=new tf::TransformBroadcaster();
00032 listener=new tf::TransformListener();
00033 ros::spin();
00034 }