0
$\begingroup$

Rosanswers logo

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: -20.6123192152
    z: -5.65655747669
  rotation: 
    x: -0.0180894607282
    y: -0.02894876557
    z: 0.0815731223518
    w: 0.996082610074
Incoming Point is
header: 
  seq: 17
  stamp: 
    secs: 1647031101
    nsecs: 449959831
  frame_id: "vehicle_flu"
point: 
  x: 270.711395264
  y: 13.0869922638
  z: 34.2443237305
New Marker
header: 
  seq: 13466
  stamp: 
    secs: 1641505401
    nsecs: 156043053
  frame_id: "relative_flu"
ns: "Bobby"
id: 69
type: 1
action: 0
pose: 
  position: 
    x: 204.400580901
    y: 37.6403387746
    z: 42.7877235921
  orientation: 
    x: 0.0
    y: 0.0
    z: 0.0
    w: 0.0
scale: 
  x: 1
  y: 1
  z: 1
color: 
  r: 0.0
  g: 1
  b: 0.0
  a: 0.5
lifetime: 
  secs: 0
  nsecs:         0
frame_locked: True
points: []
colors: []
text: ''
mesh_resource: ''
mesh_use_embedded_materials: False

Originally posted by Joshua Kordani on ROS Answers with karma: 11 on 2022-03-11

Post score: 0

$\endgroup$

1 Answer 1

0
$\begingroup$

Rosanswers logo

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).


Originally posted by Joshua Kordani with karma: 11 on 2022-03-11

This answer was ACCEPTED on the original site

Post score: 0

$\endgroup$

Your Answer

By clicking “Post Your Answer”, you agree to our terms of service and acknowledge you have read our privacy policy.