Obtain map frame pose of visual markers

asked 2020-01-28 05:41:47 -0500

kaiyu gravatar image

Here's the situation I'm facing. I have some RViz visual markers published at "robot_camera_frame". Hence, the poses of these markers are with respect to the robot's camera frame. I can see these markers move while the robot is navigating, which is as expected.

Now, I want to obtain the poses of these markers in the "map" frame. I have to do so by transforming the visual marker poses from the "robot_camera_frame" to the "map" frame. I thought this would be as straightfoward as running the following code:

if tf_listener.frameExists("robot_camera_frame")\
       and tf_listener.frameExists("map"):
        pose_stamped_msg = PoseStamped()
        pose_stamped_msg.header.frame_id = "robot_camera_frame"
        pose_stamped_msg.pose = pose_msg
        pose_stamped_transformed = tf_listener.transformPose("map", pose_stamped_msg)
        return pose_stamped_transformed.pose

However, tf_listener.frameExists("map") is False(!) I checked the existence of other frames, such as "odom", and "robot_camera_frame", etc., and they all exist. Except for "map". The strangest thing is, when I run rqt_tf_tree, I can clearly see the "map" frame being broadcasted by amcl. I couldn't understand why.

So I had to try applying the transform manually. My code is below:

    (trans,rot) = tf_listener.lookupTransform("map", "robot_camera_frame", rospy.Time(0))

    from scipy.spatial.transform import Rotation as R
    # marker pose corresponds to a visual marker in the robot_camera_frame
    rx, ry, rz = R.from_quat(rot).apply(marker_pose)   
    map_pose = (float(rx + trans[0]),
                         float(ry + trans[1]),
                         float(rz + trans[2]))

Interestingly, lookupTransform works for "map" frame (the tf listener is started a while ago, so there is no need to do waitForTransform first). However, the map_pose I obtain is wrong (when I publish markers about this map_pose at "map" frame, it does not show up at the same location as the visual marker).

What am I missing/doing wrong? Thanks A LOT!

edit retag flag offensive close merge delete