Transform of Marker in RVIZ giving unexpected results (a lesson on what /use_sim_time is for)
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 ...