common.cpp
Go to the documentation of this file.
00001 /*
00002  * common.cpp
00003  *
00004  *  Created on: Apr 11, 2013
00005  *      Author: jorge
00006  */
00007 
00008 #include "yocs_math_toolkit/common.hpp"
00009 
00010 
00011 namespace mtk
00012 {
00013 
00014 
00015 void tf2pose(const tf::Transform& tf, geometry_msgs::Pose& pose)
00016 {
00017   pose.position.x = tf.getOrigin().x();
00018   pose.position.y = tf.getOrigin().y();
00019   pose.position.z = tf.getOrigin().z();
00020   tf::quaternionTFToMsg(tf.getRotation(), pose.orientation);
00021 }
00022 
00023 void tf2pose(const tf::StampedTransform& tf, geometry_msgs::PoseStamped& pose)
00024 {
00025   pose.header.stamp    = tf.stamp_;
00026   pose.header.frame_id = tf.frame_id_;
00027   tf2pose(tf, pose.pose);
00028 }
00029 
00030 void pose2tf(const geometry_msgs::Pose& pose, tf::Transform& tf)
00031 {
00032   tf.setOrigin(tf::Vector3(pose.position.x, pose.position.y, pose.position.z));
00033   tf::Quaternion q;
00034   tf::quaternionMsgToTF(pose.orientation, q);
00035   tf.setRotation(q);
00036 }
00037 
00038 void pose2tf(const geometry_msgs::PoseStamped& pose, tf::StampedTransform& tf)
00039 {
00040   tf.stamp_    = pose.header.stamp;
00041   tf.frame_id_ = pose.header.frame_id;
00042   pose2tf(pose.pose, tf);
00043 }
00044 
00045 char ___buffer___[256];
00046 
00047 const char* point2str(const geometry_msgs::Point& point)
00048 {
00049   sprintf(___buffer___, "%.2f, %.2f, %.2f", point.x, point.y, point.z);
00050   return (const char*)___buffer___;
00051 }
00052 
00053 const char* pose2str(const geometry_msgs::Pose& pose)
00054 {
00055   sprintf(___buffer___, "%.2f, %.2f, %.2f", pose.position.x, pose.position.y, tf::getYaw(pose.orientation));
00056   return (const char*)___buffer___;
00057 }
00058 
00059 const char* pose2str(const geometry_msgs::PoseStamped& pose)
00060 {
00061   return pose2str(pose.pose);
00062 }
00063 
00064 } /* namespace mtk */


yocs_math_toolkit
Author(s): Jorge Santos
autogenerated on Fri Aug 28 2015 13:44:54