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;