odom->base_link

标签：

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <iostream>
using namespace std;
double x,y,z,ww,zz,hh,ii,Aww,Azz,Ahh,Aii;
int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_listener");

  ros::NodeHandle node;
  tf::TransformListener listener;
  ros::Rate rate(10.0);
  while (node.ok()){
    tf::StampedTransform transform;
    try{

      listener.waitForTransform("odom", "base_link", ros::Time(0), ros::Duration(1.0));
      listener.lookupTransform("odom", "base_link",
                               ros::Time(0), transform);
    }
    catch (tf::TransformException &ex) {
      ROS_ERROR("%s",ex.what());
      ros::Duration(1.0).sleep();
    }

    x=transform.getOrigin().x();
    y=transform.getOrigin().y();
    z=transform.getOrigin().z();


    cout<<"x: "<<x<<endl;
    cout<<"y: "<<y<<endl;
    cout<<"z: "<<z<<endl;



     ww=transform.getRotation()[0];
     zz=transform.getRotation()[1];
     hh=transform.getRotation()[2];
     ii=transform.getRotation()[3];
     cout<<ww<<endl;
     cout<<zz<<endl;
     cout<<hh<<endl;
     cout<<ii<<endl;

     Aww=transform.getRotation().getX();
     Azz=transform.getRotation().getY();
     Ahh=transform.getRotation().getZ();
     Aii=transform.getRotation().getW();
     cout<<Aww<<endl;
     cout<<Azz<<endl;
     cout<<Ahh<<endl;
     cout<<Aii<<endl;
//两种不同的表示方法，来表示getRotation
    rate.sleep();
  }
  return 0;
};

turtle_tf_broadcaster.cpp

#include <ros/ros.h>
#include <tf/transform_broadcaster.h>

int main(int argc, char** argv){
  ros::init(argc, argv, "my_tf_broadcaster");

  ros::NodeHandle node;

  static tf::TransformBroadcaster br;
  tf::Transform transform;
  ros::Rate r(100);

  while(node.ok()){
  transform.setOrigin( tf::Vector3(1.0, 2.0, 0.0) );


  tf::Quaternion q;
  q.setRPY(1.0, 0.2, 0.3);
  transform.setRotation(q);
  br.sendTransform(tf::StampedTransform(transform, ros::Time::now(), "odom", "base_link"));
  r.sleep();
  }

return 0;
};

odom->base_link

标签：

原文地址：http://blog.csdn.net/dxuehui/article/details/45533117