tf_echo.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <cstdio>
31 #include "tf/transform_listener.h"
32 #include "ros/ros.h"
33 
34 #define _USE_MATH_DEFINES
36 {
37 public:
38 
40 
41  //constructor with name
43  {
44 
45  };
46 
48  {
49 
50  };
51 
52 private:
53 
54 };
55 
56 
57 int main(int argc, char ** argv)
58 {
59  //Initialize ROS
60  ros::init(argc, argv, "tf_echo", ros::init_options::AnonymousName);
61 
62  // Allow 2 or 3 command line arguments
63  if (argc < 3 || argc > 4)
64  {
65  printf("Usage: tf_echo source_frame target_frame [echo_rate]\n\n");
66  printf("This will echo the transform from the coordinate frame of the source_frame\n");
67  printf("to the coordinate frame of the target_frame. \n");
68  printf("Note: This is the transform to get data from target_frame into the source_frame.\n");
69  printf("Default echo rate is 1 if echo_rate is not given.\n");
70  return -1;
71  }
72 
73  ros::NodeHandle nh;
74 
75  double rate_hz;
76  if (argc == 4)
77  {
78  // read rate from command line
79  rate_hz = atof(argv[3]);
80  }
81  else
82  {
83  // read rate parameter
84  ros::NodeHandle p_nh("~");
85  p_nh.param("rate", rate_hz, 1.0);
86  }
87  if (rate_hz <= 0.0)
88  {
89  std::cerr << "Echo rate must be > 0.0\n";
90  return -1;
91  }
92  ros::Rate rate(rate_hz);
93 
94  //Instantiate a local listener
96 
97 
98  std::string source_frameid = std::string(argv[1]);
99  std::string target_frameid = std::string(argv[2]);
100 
101  // Wait for up to one second for the first transforms to become avaiable.
102  echoListener.tf.waitForTransform(source_frameid, target_frameid, ros::Time(), ros::Duration(1.0));
103 
104  //Nothing needs to be done except wait for a quit
105  //The callbacks withing the listener class
106  //will take care of everything
107  while(nh.ok())
108  {
109  try
110  {
111  tf::StampedTransform echo_transform;
112  echoListener.tf.lookupTransform(source_frameid, target_frameid, ros::Time(), echo_transform);
113  std::cout.precision(3);
114  std::cout.setf(std::ios::fixed,std::ios::floatfield);
115  std::cout << "At time " << echo_transform.stamp_.toSec() << std::endl;
116  double yaw, pitch, roll;
117  echo_transform.getBasis().getRPY(roll, pitch, yaw);
118  tf::Quaternion q = echo_transform.getRotation();
119  tf::Vector3 v = echo_transform.getOrigin();
120  std::cout << "- Translation: [" << v.getX() << ", " << v.getY() << ", " << v.getZ() << "]" << std::endl;
121  std::cout << "- Rotation: in Quaternion [" << q.getX() << ", " << q.getY() << ", "
122  << q.getZ() << ", " << q.getW() << "]" << std::endl
123  << " in RPY (radian) [" << roll << ", " << pitch << ", " << yaw << "]" << std::endl
124  << " in RPY (degree) [" << roll*180.0/M_PI << ", " << pitch*180.0/M_PI << ", " << yaw*180.0/M_PI << "]" << std::endl;
125 
126  //print transform
127  }
128  catch(tf::TransformException& ex)
129  {
130  std::cout << "Failure at "<< ros::Time::now() << std::endl;
131  std::cout << "Exception thrown:" << ex.what()<< std::endl;
132  std::cout << "The current list of frames is:" <<std::endl;
133  std::cout << echoListener.tf.allFramesAsString()<<std::endl;
134 
135  }
136  rate.sleep();
137  }
138 
139  return 0;
140 };
141 
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:28
TFSIMD_FORCE_INLINE Matrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: Transform.h:110
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
TFSIMD_FORCE_INLINE const tfScalar & getY() const
Return the y value.
Definition: Vector3.h:251
std::string allFramesAsString() const
A way to see what frames have been cached Useful for debugging.
Definition: tf.cpp:434
ros::Time stamp_
The timestamp associated with this transform.
TFSIMD_FORCE_INLINE const tfScalar & getW() const
Definition: Quaternion.h:346
TFSIMD_FORCE_INLINE const tfScalar & getZ() const
Return the z value.
Definition: Vector3.h:253
bool waitForTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
Block until a transform is possible or it times out.
Definition: tf.cpp:348
void getRPY(tfScalar &roll, tfScalar &pitch, tfScalar &yaw, unsigned int solution_number=1) const
Get the matrix represented as roll pitch and yaw about fixed axes XYZ.
Definition: Matrix3x3.h:365
bool param(const std::string &param_name, T &param_val, const T &default_val) const
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
Return the origin vector translation.
Definition: Transform.h:115
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
Get the transform between two frames by frame ID.
Definition: tf.cpp:238
bool sleep()
This class inherits from Transformer and automatically subscribes to ROS transform messages...
Quaternion getRotation() const
Return a quaternion representing the rotation.
Definition: Transform.h:120
tf::TransformListener tf
Definition: tf_echo.cpp:39
TFSIMD_FORCE_INLINE const tfScalar & getX() const
Return the x value.
Definition: Vector3.h:249
static Time now()
bool ok() const
int main(int argc, char **argv)
Definition: tf_echo.cpp:57
The Stamped Transform datatype used by tf.
Vector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-byte...
Definition: Vector3.h:38


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Fri Jun 7 2019 22:00:28