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  nh.param("rate", rate_hz, 1.0);
85  }
86  if (rate_hz <= 0.0)
87  {
88  std::cerr << "Echo rate must be > 0.0\n";
89  return -1;
90  }
91  ros::Rate rate(rate_hz);
92 
93  int precision(3);
94  if (nh.getParam("precision", precision))
95  {
96  if (precision < 1)
97  {
98  std::cerr << "Precision must be > 0\n";
99  return -1;
100  }
101  printf("Precision default value was overriden, new value: %d\n", precision);
102  }
103 
104  //Instantiate a local listener
106 
107 
108  std::string source_frameid = std::string(argv[1]);
109  std::string target_frameid = std::string(argv[2]);
110 
111  // Wait for up to one second for the first transforms to become avaiable.
112  echoListener.tf.waitForTransform(source_frameid, target_frameid, ros::Time(), ros::Duration(1.0));
113 
114  //Nothing needs to be done except wait for a quit
115  //The callbacks withing the listener class
116  //will take care of everything
117  while(nh.ok())
118  {
119  try
120  {
121  tf::StampedTransform echo_transform;
122  echoListener.tf.lookupTransform(source_frameid, target_frameid, ros::Time(), echo_transform);
123  std::cout.precision(precision);
124  std::cout.setf(std::ios::fixed,std::ios::floatfield);
125  std::cout << "At time " << echo_transform.stamp_.toSec() << std::endl;
126  double yaw, pitch, roll;
127  echo_transform.getBasis().getRPY(roll, pitch, yaw);
128  tf::Quaternion q = echo_transform.getRotation();
129  tf::Vector3 v = echo_transform.getOrigin();
130  std::cout << "- Translation: [" << v.getX() << ", " << v.getY() << ", " << v.getZ() << "]" << std::endl;
131  std::cout << "- Rotation: in Quaternion [" << q.getX() << ", " << q.getY() << ", "
132  << q.getZ() << ", " << q.getW() << "]" << std::endl
133  << " in RPY (radian) [" << roll << ", " << pitch << ", " << yaw << "]" << std::endl
134  << " in RPY (degree) [" << roll*180.0/M_PI << ", " << pitch*180.0/M_PI << ", " << yaw*180.0/M_PI << "]" << std::endl;
135 
136  //print transform
137  }
138  catch(tf::TransformException& ex)
139  {
140  std::cout << "Failure at "<< ros::Time::now() << std::endl;
141  std::cout << "Exception thrown:" << ex.what()<< std::endl;
142  std::cout << "The current list of frames is:" <<std::endl;
143  std::cout << echoListener.tf.allFramesAsString()<<std::endl;
144 
145  }
146  rate.sleep();
147  }
148 
149  return 0;
150 }
151 
std::string allFramesAsString() const
A way to see what frames have been cached Useful for debugging.
Definition: tf.cpp:433
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
Definition: Quaternion.h:30
Quaternion getRotation() const
Return a quaternion representing the rotation.
Definition: Transform.h:120
TFSIMD_FORCE_INLINE Matrix3x3 & getBasis()
Return the basis matrix for the rotation.
Definition: Transform.h:110
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:347
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
ros::Time stamp_
The timestamp associated with this transform.
bool param(const std::string &param_name, T &param_val, const T &default_val) const
TFSIMD_FORCE_INLINE const tfScalar & getW() const
Definition: Quaternion.h:348
bool getParam(const std::string &key, std::string &s) const
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
Return the origin vector translation.
Definition: Transform.h:115
bool sleep()
This class inherits from Transformer and automatically subscribes to ROS transform messages...
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:237
tf::TransformListener tf
Definition: tf_echo.cpp:39
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:367
static Time now()
int main(int argc, char **argv)
Definition: tf_echo.cpp:57
bool ok() const
The Stamped Transform datatype used by tf.


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Mon Feb 28 2022 22:26:19