0
$\begingroup$

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.

enter image description here

$\endgroup$

1 Answer 1

0
$\begingroup$

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: RVIZ Image

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.

$\endgroup$
6
  • $\begingroup$ 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. $\endgroup$
    – aotombo
    Jan 30 at 14:21
  • $\begingroup$ Please see my edited answer $\endgroup$
    – ÁngeLoGa
    Jan 30 at 14:28
  • $\begingroup$ Thank you, @ÁngeLoGa. Stating the tool to use is good, but explaining how to do it would be the answer. Assume a case where a camera shot is captured. What calculations should be done to end up with the value of the camera pose in the world frame? $\endgroup$
    – Robotawi
    Jan 31 at 15:21
  • $\begingroup$ You are right @Robotawi, it is just that I don't have much spare time, and as I am a bit scared of answering and commenting because of my score and reputation in Roboticks Stack...hehe, please see my edited answer $\endgroup$
    – ÁngeLoGa
    Feb 1 at 7:46
  • $\begingroup$ Hi, AngeLoGa. I am so sorry for my late response. I had bloody stool, then my company let me keep away from the computer for a while. Thank you for your edited answer. I check ASAP. Thank you. $\endgroup$
    – aotombo
    Feb 9 at 1:40

Your Answer

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

Not the answer you're looking for? Browse other questions tagged or ask your own question.