ROS tf变换之得到两个坐标之间的转换关系

最新推荐文章于 2024-08-15 23:15:00 发布

Youngps99

最新推荐文章于 2024-08-15 23:15:00 发布

阅读量4.1k

点赞数 3

分类专栏： ROS

本文链接：https://blog.csdn.net/qq_39584578/article/details/94880911

版权

ROS 专栏收录该内容

11 篇文章 2 订阅

订阅专栏

１、two_tf_transform.cpp

#include <ros/ros.h>
#include <tf/transform_listener.h>
#include <geometry_msgs/Twist.h>
#include <turtlesim/Spawn.h>
#include <iostream>
#include <cmath>
using namespace std;
double x,y,z,ww,zz,hh,ii,Aww,Azz,Ahh,Aii;
double theta;
int main(int argc, char** argv){
  ros::init(argc, argv, "two_tf_transform");
 
  ros::NodeHandle node;
  tf::TransformListener listener;
  ros::Rate rate(10.0);
  while (node.ok()){
    tf::StampedTransform transform;
    try{
        //得到坐标odom和坐标base_link之间的关系
      listener.waitForTransform("odom", "base_link", ros::Time(0), ros::Duration(3.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;
    //两种不同的表示方法，来表示getRotation
     ww=transform.getRotation()[0];
     zz=transform.getRotation()[1];
     hh=transform.getRotation()[2];
     ii=transform.getRotation()[3];
     cout<<"ww: "<<ww<<endl;
     cout<<"zz: "<<zz<<endl;
     cout<<"hh: "<<hh<<endl;
     cout<<"ii: "<<ii<<endl;
     Aww=transform.getRotation().getX();
     Azz=transform.getRotation().getY();
     Ahh=transform.getRotation().getZ();
     Aii=transform.getRotation().getW();
     theta=2*acos(transform.getRotation().getW())/3.1415926*180;
     cout<<"Aww: "<<Aww<<endl;
     cout<<"Azz: "<<Azz<<endl;
     cout<<"Ahh: "<<Ahh<<endl;
     cout<<"Aii: "<<Aii<<endl;
     cout<<"Theta: "<<theta<<endl;
     cout<<endl;

    rate.sleep();
  }
  return 0;
};

２、CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(learn_tf)


find_package(catkin REQUIRED COMPONENTS
  geometry_msgs
  roscpp
  tf
)


catkin_package(

)


## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(
# include
  ${catkin_INCLUDE_DIRS}
)


add_executable(two_tf_transform src/two_tf_transform.cpp)
target_link_libraries(two_tf_transform ${catkin_LIBRARIES})

３、package.xml

<?xml version="1.0"?>
<package format="2">
  <name>learn_tf</name>
  <version>0.0.0</version>
  <description>The learn_tf package</description>

  <maintainer email="patience@todo.todo">patience</maintainer>
  <license>TODO</license>


  <buildtool_depend>catkin</buildtool_depend>
  <build_depend>geometry_msgs</build_depend>
  <build_depend>roscpp</build_depend>
  <build_depend>tf</build_depend>
  <build_export_depend>geometry_msgs</build_export_depend>
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>tf</build_export_depend>
  <exec_depend>geometry_msgs</exec_depend>
  <exec_depend>roscpp</exec_depend>
  <exec_depend>tf</exec_depend>


  <!-- The export tag contains other, unspecified, tags -->
  <export>
    <!-- Other tools can request additional information be placed here -->

  </export>
</package>