-
Notifications
You must be signed in to change notification settings - Fork 43
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Camera frames do not correspond to the docs #33
Comments
Hi Kim, |
Just to be clear, what I'm doing here is using the following script to publish frames on import rospy
import rostopic
import geometry_msgs.msg
import tf2_ros
class Listener:
def __init__(self, topic):
self.msg = None
msg_type = rostopic.get_topic_class(topic)[0]
self.sub = rospy.Subscriber(topic, msg_type, self.set_msg)
def set_msg(self, msg):
self.msg = msg
def wait_for_message(topic):
msg_type = rostopic.get_topic_class(topic)[0]
return rospy.wait_for_message(topic, msg_type)
def pose_to_transform(pose):
tf = geometry_msgs.msg.Transform()
tf.translation.x = pose.position.x
tf.translation.y = pose.position.y
tf.translation.z = pose.position.z
tf.rotation.w = pose.orientation.w
tf.rotation.x = pose.orientation.x
tf.rotation.y = pose.orientation.y
tf.rotation.z = pose.orientation.z
return tf
rospy.init_node('ambf_state_publisher')
ambf_static = {
'psm1/baselink': wait_for_message('/ambf/env/psm1/baselink/State'),
'psm2/baselink': wait_for_message('/ambf/env/psm2/baselink/State'),
}
ambf_moving = {
'Needle': Listener('/ambf/env/Needle/State'),
'CameraFrame': Listener('/ambf/env/CameraFrame/State'),
'cameraL': Listener('/ambf/env/cameras/cameraL/State'),
'cameraR': Listener('/ambf/env/cameras/cameraR/State'),
'Entry1': Listener('/ambf/env/Entry1/State'),
'Entry2': Listener('/ambf/env/Entry2/State'),
'Entry3': Listener('/ambf/env/Entry3/State'),
'Entry4': Listener('/ambf/env/Entry4/State'),
'Exit1': Listener('/ambf/env/Exit1/State'),
'Exit2': Listener('/ambf/env/Exit2/State'),
'Exit3': Listener('/ambf/env/Exit3/State'),
'Exit4': Listener('/ambf/env/Exit4/State'),
# 'psm1/toolyawlink': Listener('/ambf/env/psm1/toolyawlink/State'),
# 'psm2/toolyawlink': Listener('/ambf/env/psm2/toolyawlink/State'),
}
tf_static_broadcaster = tf2_ros.StaticTransformBroadcaster()
tf_broadcaster = tf2_ros.TransformBroadcaster()
transforms = []
for k, v in ambf_static.items():
tf = geometry_msgs.msg.TransformStamped()
tf.header.stamp = rospy.Time.now()
tf.header.frame_id = 'world'
tf.child_frame_id = k
tf.transform = pose_to_transform(v.pose)
transforms.append(tf)
tf_static_broadcaster.sendTransform(transforms)
rate = rospy.Rate(30)
while not rospy.is_shutdown():
transforms = []
for k, v in ambf_moving.items():
if v.msg is None:
continue
tf = geometry_msgs.msg.TransformStamped()
tf.header.stamp = rospy.Time.now()
try:
tf.header.frame_id = v.msg.parent_name.data.replace('BODY ', '')
except AttributeError:
tf.header.frame_id = 'world'
tf.child_frame_id = k
tf.transform = pose_to_transform(v.msg.pose)
transforms.append(tf)
tf_broadcaster.sendTransform(transforms)
rate.sleep() |
Thanks for posting the code for visualizing the frames. I want to check what specific part of the camera_conventions.md do you think is incorrect. |
Yes, if I'm not mistaken, the one you posted is rotated 180 deg about the Z axis compared to the actual one from the simulator. |
Umm, just so that we are on the same page, the conventions depicted in the two images are not related. This image: shows the On the other hand, this image that you shared shows the |
Oooohhh, wait! I now realize I was reading the figure wrong. Stupid me. Sorry to have you spend time on this :| |
No problem, happy to help. :) |
Hi,
I believe the
CameraL
andCameraR
frames do not correspond to what's in the docs -- they are rotated 180 degrees about their Z axis (which is pointing upward correctly).I have visualized the frames in RViz to confirm my suspicion:
![image](https://user-images.githubusercontent.com/821793/161625135-7c26599f-9e9d-42d4-9230-4d7e52a6f081.png)
The text was updated successfully, but these errors were encountered: