粗大メモ置き場

個人用,たまーに来訪者を意識する雑記メモ

ROS tf and geometry_msgs mutual translation

I tried convert tf::Pose to geomtery_msgs and memorize it in here.
But I don't think this is best way.
tf looks great.
tf/Tutorials - ROS Wiki

tf::Quaternion to geometry_msgs::Pose

geometry_msgs::Quaternion  msgqt;
tf::quaternionTFToMsg(qt, msgqt);

geometry_msgs::PoseStamped to tf::Transform

const geometry_msgs::PoseStamped::ConstPtr& Pose
tf::Transform pose;
tf::poseMsgToTF(Pose->pose,pose);

tf::Transform to tf::Matrix3x3

tf::Matrix3x3 Rot;
Rot = pose.getBasis();

get each element of tf::Matrix3x3

std::cout <<  Rot.getRow(0).getX() << Rot.getRow(0).getY() << Rot.getRow(0).getZ() << std::endl; 
std::cout <<  Rot.getRow(1).getX() << Rot.getRow(1).getY() << Rot.getRow(1).getZ() << std::endl; 
std::cout <<  Rot.getRow(2).getX() << Rot.getRow(2).getY() << Rot.getRow(2).getZ() << std::endl; 
広告を非表示にする