坐标转换失败

[Chengyawen26]

我是ros2 humble系统, 我想要把相机坐标系下的物体点转换到地图坐标系下，但是一直转换失败。

我验证了相机坐标系和map之间是能够转换的，请问该如何把相机下的点转到map下呢？

主要就是： transformed_pose = do_transform_pose(pose_stamped, transform) 出错

import rclpy
from tf2_ros import Buffer, TransformListener
from tf2_ros.buffer_interface import BufferInterface
from tf2_geometry_msgs import do_transform_pose
from geometry_msgs.msg import PoseStamped
import tf_transformations

def main():
    rclpy.init()

    node = rclpy.create_node('tf2_listener')

    tf_buffer = Buffer()
    listener = TransformListener(tf_buffer, node)

    # pose
    pose_stamped = PoseStamped()
    pose_stamped.header.frame_id = "camera"
    pose_stamped.pose.position.x = 1.0
    pose_stamped.pose.position.y = 2.0
    pose_stamped.pose.position.z = 3.0
    pose_stamped.pose.orientation.x = 0.0
    pose_stamped.pose.orientation.y = 0.0
    pose_stamped.pose.orientation.z = 0.0
    pose_stamped.pose.orientation.w = 1.0

    rclpy.spin_once(node, timeout_sec=1)  

    transform = tf_buffer.lookup_transform('base_link', 'camera', rclpy.time.Time())
    print(transform)

    transformed_pose = do_transform_pose(pose_stamped, transform)

    print(transformed_pose)

    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

报错结果:

Traceback (most recent call last):
  File "/home/humble/fishbot_ws/install/tflola/lib/tflola/test2", line 33, in <module>
    sys.exit(load_entry_point('tflola', 'console_scripts', 'test2')())
  File "/home/humble/fishbot_ws/build/tflola/tflola/test2.py", line 79, in main
    transformed_pose = do_transform_pose(pose_stamped, transform)
  File "/opt/ros/humble/local/lib/python3.10/dist-packages/tf2_geometry_msgs/tf2_geometry_msgs.py", line 352, in do_transform_pose
    pose.position.x,
AttributeError: 'PoseStamped' object has no attribute 'position'
[ros2run]: Process exited with failure 1

请大家不吝赐教

[小鱼]

@Chengyawen26 chatgpt

根据你提供的代码和错误信息，报错显示 'PoseStamped' 对象没有 'position' 属性。这表明您的 pose_stamped 对象并没有正确设置，因此无法访问其位置信息。在设置 pose_stamped 时，请确保正确设置了其位置信息。

您可以尝试更改您的代码，如下所示：

import rclpy
from tf2_ros import Buffer, TransformListener
from tf2_geometry_msgs import do_transform_pose
from geometry_msgs.msg import PoseStamped, TransformStamped
import tf2_ros

def main():
    rclpy.init()

    node = rclpy.create_node('tf2_listener')

    tf_buffer = Buffer()
    listener = TransformListener(tf_buffer, node)

    # pose
    pose_stamped = PoseStamped()
    pose_stamped.header.frame_id = "camera"
    pose_stamped.pose.position.x = 1.0
    pose_stamped.pose.position.y = 2.0
    pose_stamped.pose.position.z = 3.0
    pose_stamped.pose.orientation.x = 0.0
    pose_stamped.pose.orientation.y = 0.0
    pose_stamped.pose.orientation.z = 0.0
    pose_stamped.pose.orientation.w = 1.0

    rclpy.spin_once(node, timeout_sec=1)  

    try:
        transform = tf_buffer.lookup_transform('base_link', 'camera', rclpy.time.Time())
        transformed_pose = do_transform_pose(pose_stamped, transform)
        print(transformed_pose)
    except tf2_ros.LookupException as e:
        print("Transform not available: ", e)
    except tf2_ros.ExtrapolationException as e:
        print("Extrapolation exception: ", e)

    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()

在此示例中，我稍微更改了导入，添加了异常处理以捕获转换查找可能出现的异常，并在异常处理块中打印相关错误信息。请确保您的 TF2 树中确实存在 'camera' 到 'base_link' 的变换。

[aweliant]

这里应该使用do_transform_pose_stamped。在opt/ros/humble/local/lib/python3.10/dist-packages/tf2_geometry_msgs/tf2_geometry_msgs.py可以看到源代码。