34 #define _USE_MATH_DEFINES 57 int main(
int argc,
char ** argv)
63 if (argc < 3 || argc > 4)
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");
79 rate_hz = atof(argv[3]);
85 p_nh.
param(
"rate", rate_hz, 1.0);
89 std::cerr <<
"Echo rate must be > 0.0\n";
98 std::string source_frameid = std::string(argv[1]);
99 std::string target_frameid = std::string(argv[2]);
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;
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;
131 std::cout <<
"Exception thrown:" << ex.what()<< std::endl;
132 std::cout <<
"The current list of frames is:" <<std::endl;
The Quaternion implements quaternion to perform linear algebra rotations in combination with Matrix3x...
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.
TFSIMD_FORCE_INLINE const tfScalar & getW() const
TFSIMD_FORCE_INLINE const tfScalar & getZ() const
Return the z value.
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.
bool param(const std::string ¶m_name, T ¶m_val, const T &default_val) const
TFSIMD_FORCE_INLINE const tfScalar & getX() const
Return the x value.
int main(int argc, char **argv)
Vector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-byte...