# How to get the world coordinates with position and orientation in ROS 2 Humble

I am using ROS 2 Humble. A person is supposed to take a photo of an AR marker with a camera. Inside the system, we can obtain the relative coordinates, position (x, y, z), and rotation, orientation (x, y, z, w). Then, I want to get the world coordinates of the camera shooter, but I don't know how to do it.

Let's assume:

• The world coordinates of the world origin are (0.5, 0.5, 0.5).
• The world coordinates of the AR marker are (0.810, 2.450, 1.430).
• The output position is (-0.326, 0.125, 0.892).
• The output orientation is (0.266, 0.237, -0.695, 0.661).
• Units are in meters.

In this case, how should I code it with Python?

I wrote the following code, but I think it's incorrect. If there are any corrections or suggestions, please let me know. Thank you in advance.

PS. I am sorry for my weird English.

import rclpy
from std_msgs.msg import String
from rclpy.node import Node

import math
import numpy as np
import tf2_ros
import tf_transformations as tf

def tag_detections_callback(self, msg):
px = -0.326
py = 0.125
pz = 0.892
ox = 0.266
oy = 0.237
oz = -0.695
ow = 0.661

# クォータニオン
quaternion = [ox, oy, oz, ow]
# 位置
position = [px, py, pz]

# Local <= I think around here is incorrect
lx = 0.810
ly = 2.450
lz = 1.430
local_point = np.array([lx, ly, lz, 1])

# 座標変換行列を作成
# クォータニオンから回転行列を作成
rotation_matrix = tf.quaternion_matrix(quaternion)
# 位置を設定
rotation_matrix[:3, 3] = np.dot(rotation_matrix[:3, :3], position)
# ローカル座標系の点をワールド座標系に変換
world_point = np.dot(rotation_matrix, local_point)  # 内積計算

self.get_logger().info(f"World coordinates: {world_point[:3]}")


In this image, the red line is what I need to calculate.

Hello and welcome to Robotics Stack Exchange! I think one way to handle this is through tfs. May I ask if you have some tfs set for this? In case so, you could use tf2 or tf in order to obtain the transformation between both frames "World Origin" and "Camera Shooter", which you could use to calculate the position of the Camera Shooter.

Edit: Sure, don't worry, we all have been there! I encourage you to check these links: http://wiki.ros.org/tf/Tutorials and http://wiki.ros.org/tf2/Tutorials. The first one is about tf, and the second one tf2. This is a key factor to ROS in general, please read it carefully and understand the purpose of the package.

In resume, TF provides you a way to publish coordinate frame axis in order to use them to calculate what you are asking. You could change between one frame and another using the transform between them, compossed by a traslation and a rotation.

I would love to keep explaining stuff, but I think the best way to start with ROS / ROS 2 is digging in documentation and testing it by yourself. If you give a try to the tutorials, and still need help, do not hesitate to ask, but as I mentioned, give it a try first!

BTW, if this answer is useful for you, I would thank you you select it as "Answer" in order to improve and be able to help more people like you.

Edit2: Alright, let's sum up the problem this would be a graphical representation of the example you gave:

First of all, I would recommend you fix to the REP 105 (https://www.ros.org/reps/rep-0105.html) in order to publish tfs as close as possible to the standard. Here we published the transform from "world" to "camera_shooter" by a static publisher. We published as well a Marker msg in order to present a point representing the AR Marker.

tf has functions such as lookUpTransform(https://docs.ros.org/en/indigo/api/tf/html/c++/classtf_1_1Transformer.html#ac01a9f8709a828c427f1a5faa0ced42b) that inserting two frames gives you the transformation between both of them. That would give us the red arrow you asked in your image. From the tutorials I mentioned before:

        try:
(trans,rot) = self.tf_listener.lookupTransform('/world', '/camera_shooter', rospy.Time(0))
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
pass


If the idea is to get the coordinates of the point in the coordinate frame "world" instead of "camera_shooter", you could use transformPoint instead (there's a bunch more for Pose, PointCloud, etc.).

In order to do this is as simple as creating a listener first, and then using the function giving the target_frame and the point you want to transform:

        self.tf_listener = tf.TransformListener()
self.tf_listener.waitForTransform("/world","camera_shooter", rospy.Time(0),rospy.Duration(1.0))
new_point = self.tf_listener.transformPoint("world", point)


If you need the code or more help feel free to ask for it.

• Hi, ÁngeLoGa. Thank you for your reply. I am a new of ROS and ROS2, so I am not sure if I have tfs set or not, but I can use import tf2_ros and import tf_transformations as tf. If I misunderstand what you wrote, please let me know. Thank you. Jan 30 at 14:21