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

Transform of Marker in RVIZ giving unexpected results (a lesson on what /use_sim_time is for)

asked 2022-03-11 14:36:46 -0500

Joshua Kordani gravatar image

updated 2022-03-11 19:31:53 -0500

EDIT I think that the transform is working as expected (in that I get the values I expect for the transform) but when rviz does the transformation for visualization, it is put in the wrong place. When the fixed frame is set to relative_flu, the marker stays on top of the point that was clicked over time. When the fixed frame is anything else, not only is the marker in the wrong place, but the marker stays fixed relative to the frame center

I am writing a small python ROS node to take a clicked point from rviz and make a marker from it. I ultimately want to store the coordinates of the marker in a specific frame no matter which frame the clicked point comes from. Its great that the args for the source and destination frames are reversed in the python api. But for fun I swap them and it still doesn't work. Code and example output below. I am trying to go from the frame of the clicked point to the frame I want, but I need rviz to display the marker in the fixed frame. I want to store the coordinates transformed into the to frame. I set the time of the marker I publish from the timestamp of the transform. I'm not sure why the coordinates for the published marker don't get transformed in rviz correctly

#!/usr/bin/env python2
import rospy
from visualization_msgs.msg import Marker
from geometry_msgs.msg import PointStamped

import tf2_geometry_msgs
import tf2_ros

def cb(msg):
    myMarker.action = Marker.MODIFY

    try:
        trans = tfBuffer.lookup_transform("relative_flu", msg.header.frame_id, rospy.Time())
        print("Trans is")
        print(trans)

        print("Incoming Point is")
        print(msg)

        myMarker.header.stamp = trans.header.stamp

        relpose = tf2_geometry_msgs.do_transform_point(PointStamped(point=msg.point),trans).point

        myMarker.pose.position.x = relpose.x
        myMarker.pose.position.y = relpose.y
        myMarker.pose.position.z = relpose.z

        print("New Marker")
        print(myMarker)

    except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
        print("missed transform, click again")
        raise
        r.sleep()

rospy.init_node("marker")
myMarker = Marker()


myMarker.type = Marker.CUBE
myMarker.action = Marker.ADD
myMarker.header.frame_id = "relative_flu"
myMarker.frame_locked = True

myMarker.pose.position.x = 1
myMarker.pose.position.y = 1
myMarker.pose.position.z = 1

myMarker.scale.x = 1
myMarker.scale.y = 1
myMarker.scale.z = 1

myMarker.color.a = 0.5
myMarker.color.g = 1


myMarker.id = 69
myMarker.ns = "Bobby"

MarkerPub = rospy.Publisher("visualization_marker", Marker,queue_size=1)
PointSub  = rospy.Subscriber("/clicked_point", PointStamped, cb)

tfBuffer = tf2_ros.Buffer()
tf2Listener = tf2_ros.TransformListener(tfBuffer)

r = rospy.Rate(10)

MarkerPub.publish(myMarker)

while not rospy.is_shutdown():

    # rospy.wait_for_message("/clicked_point", PointStamped)

    rospy.loginfo_throttle(10, "Marker pose is {}".format(myMarker.pose.position))

    MarkerPub.publish(myMarker)

    r.sleep()

Output of clicked point rostopic echo /clicked_point header: seq: 17 stamp: secs: 1647031101 nsecs: 449959831 frame_id: "vehicle_flu" point: x: 270.711395264 y: 13.0869922638 z: 34.2443237305 ---

Output of transform and marker publishing

Trans is
header: 
  seq: 0
  stamp: 
    secs: 1641505401
    nsecs: 156043053
  frame_id: "vehicle_flu"
child_frame_id: "relative_flu"
transform: 
  translation: 
    x: -58.065382738
    y ...
(more)
edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted
0

answered 2022-03-11 19:31:26 -0500

Joshua Kordani gravatar image

updated 2022-03-12 17:03:24 -0500

It appears that the use of /use_sim_time makes rviz behave the way I expect, and that it's important for this to be set before you start running every node I'm leaving the below as a description of what goes wrong if you don't set /use_sim_time true and specify rosbag play --clock when replaying rosbags. With /use_sim_time true (and nodes that respect this value) ros time infrastructure will use a clock that is driven by an outside process, and not system time. rosbag play --clock sets this "sim" time to be the timestamps in the rosbag, one effect of which is that transforms build by the rosbag will be done relative to the tf messages published in the bag (and affect anything else that uses ros time, like a throttled log message, etc) What follows is left for historical purposes and describes what can go wrong when the above is not used

The issue is visualization in rviz. I am replaying a rosbag with pointclouds and tf messages. On the first playthrough, with rviz started for the first time, everything works as expected. If I leave rviz open and replay the data, if the fixed frame is anything other than relative_flu, the marker will be in the wrong place. Specifically, it will be at the last position it was transformed to at the end of the bag. I don't fully understand how tf works, and what rosbag play --clock is supposed to do. What I expected that was that, with a fresh start of rviz and the rosbag, that any click I made would capture the timestamp of the transform into the header. Any time after that, any time rviz would need to transform the point, it would use the most recent transform just prior to the time iin the message header of the marker message.

Instead It appears that it uses the latest transform after that point. What I mean by (that it has seen) is that if I play the rosbag first all the way through, the tf tree has transforms for some number of old messages starting from the most recent going back. However, when I play rviz for the first time and replay the bag, rviz looks up the transform for the marker at the timestamp of the marker message, (and if its too early, as in I clicked the point somewhere into the prior run) the transform will fail until there is a transform for that time. After this point it will continue transforming until it has reached the end of the playback. If I simply replay the rosbag, the the last transform is used to transform the marker regardless of its timestamp (which I verified is not changing on each publish).

edit flag offensive delete link more
0

answered 2022-03-11 15:51:47 -0500

tfoote gravatar image

The python API does not have the source and destination transforms reversed. It uses very specific nomenclature that you're getting very loose with referring to "destination" "to frame" and others. If you stick with the nomenclature that the transform will transform data from the "source" frame to the "target" frame it's all consistent. Note that this is the inverse of the transform of the coordinate frame from source frame to target frame, note that it's different if you're operating on the data in a reference frame versus moving the reference frame.

TO that end you can likely simplify your code by just using

relpose = tfBuffer.transform(msg, 'relative_flu')

Where it will do all the transform logic for you automatically using the header. And it will also more correctly use the timestamp from the message received instead of using Time() which will zero constructu and which implies latest which depending on your system may be non-trivially different.

You don't clarify what's wrong, and looking at your code it does what you've programmed it to do.

I'm not sure why the coordinates for the published marker don't get transformed in rviz correctly

For more help you'll need to clarify what you expect and what you're getting that's different.

edit flag offensive delete link more

Comments

python tutorial section 1.1, lists arg 1 as "from" and 2 as "to" without listing target and source

c++ tutorial section 1.2 says arg 1 is "to target frame" and 2 is "from source frame"

I just now found the code on github which calls the arguments as target and source, matching the c++ api.

I'll add clarifying remarks

Joshua Kordani gravatar image Joshua Kordani  ( 2022-03-11 16:15:48 -0500 )edit

That tutorial is not computing the transform to apply to data, it is computing the transform of the coordinate frame to then do more logic, which is why it uses the to/from pairing instead of target/source. There's unfortunately no clean way to avoid the duality of the references.

tfoote gravatar image tfoote  ( 2022-03-11 16:28:09 -0500 )edit

thanks for clearing that up

Joshua Kordani gravatar image Joshua Kordani  ( 2022-03-12 16:55:45 -0500 )edit

Question Tools

2 followers

Stats

Asked: 2022-03-11 14:36:46 -0500

Seen: 512 times

Last updated: Mar 12 '22