ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

from PoseStamped message to tf StampedTransform

asked 2011-06-19 23:12:53 -0500

quimnuss gravatar image

updated 2014-01-28 17:09:53 -0500

ngrennan gravatar image

What's the neatest way to change a PoseStamped message into a StampedTransform to be published through a transform_broadcaster?

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted

answered 2011-06-19 23:29:40 -0500

updated 2017-03-22 12:54:18 -0500

Have a look at those:

name: tf::poseStampedTFToMsg
cmd: /^static inline void poseStampedTFToMsg(const Stamped<Pose>& bt, geometry_msgs::PoseStamped & msg)$/
namespace: tf
signature: (const Stamped<Pose>& bt, geometry_msgs::PoseStamped & msg)
filename: /opt/ros/diamondback/stacks/geometry/tf/include/tf/transform_datatypes.h

name: tf::poseStampedMsgToTF
cmd: /^static inline void poseStampedMsgToTF(const geometry_msgs::PoseStamped & msg, Stamped<Pose>& bt)$/
namespace: tf
signature: (const geometry_msgs::PoseStamped & msg, Stamped<Pose>& bt)
filename: /opt/ros/diamondback/stacks/geometry/tf/include/tf/transform_datatypes.h

This is code I used a while ago for the inverse transformation:

  tf::Transform grasp_tf(gripperRotation, gripperOrigin);
  tf::Stamped<tf::Pose> grasp_tf_pose(grasp_tf);
  geometry_msgs::PoseStamped msg;
  tf::poseStampedTFToMsg    (grasp_tf_pose, msg);


I just needed to to that exact thing, so I mixed tf::poseStampedMsgToTF and tf::TransformStampedMsgToTF

namespace //anonymous
using namespace tf;
inline void myPoseMsgToTF(const geometry_msgs::Pose& msg, Transform& bt)
  bt = Transform(Quaternion(msg.orientation.x,
                 Vector3(msg.position.x, msg.position.y, msg.position.z));

///Sets the child frame fixed to "camera"
inline void myPoseStampedMsgToTF(const geometry_msgs::PoseStamped & msg, StampedTransform& bt)
  myPoseMsgToTF(msg.pose, bt); 
  bt.stamp_ = msg.header.stamp;
  bt.frame_id_ = msg.header.frame_id;
  bt.child_frame_id_ = "camera";
  ROS_WARN_ONCE("You are using a path which does not carry the information about the "
                "name of the sensor frame. Using 'camera' as child_frame_id.\n"
                "Make sure the positions in your path are those of the 'camera' frame.");
}//anon namespace
edit flag offensive delete link more


I find these easy to use.

aknirala gravatar image aknirala  ( 2013-04-12 19:54:19 -0500 )edit

Does this still work in ROS Indigo?

shamitb gravatar image shamitb  ( 2016-03-07 06:34:49 -0500 )edit

I don't know. In Indigo you should probably use tf2 anyway.

Felix Endres gravatar image Felix Endres  ( 2016-03-09 12:32:45 -0500 )edit

answered 2011-06-20 01:58:39 -0500

quimnuss gravatar image

From your suggestion, I couldn't find a satisfactory way to change from tf::Stamped<tf::pose> to tf::PoseStamped, and it was getting too confusing with bullet, so I just went for the easy, less-elegant:

tf::Transform transform_auxiliar;
transform_auxiliar.setOrigin(tf::Vector3(pose_msg.pose.position.x, pose_msg.pose.position.y, pose_msg.pose.position.z));
transform_auxiliar.setRotation(tf::Quaternion( pose_msg.pose.position.x, pose_msg.pose.orientation.y, pose_msg.pose.orientation.z, pose_msg.pose.orientation.w));

tf_br.sendTransform(tf::StampedTransform(transform_auxiliar, ros::Time::now(), "openni_camera", "graspingPoint"));

Thanks, though.

edit flag offensive delete link more


I think in line 3, for "tf::Quaternion", the first argument should be "orientation.x" instead of "position.x", right?
alfa_80 gravatar image alfa_80  ( 2011-11-02 04:40:02 -0500 )edit

Question Tools



Asked: 2011-06-19 23:12:53 -0500

Seen: 16,620 times

Last updated: Mar 22 '17