Inaccurate result from tf2 transform
Platform: ubuntu 20.02 ROS2: Galactic, installed through 'apt'
ROS environment : ROS_VERSION=2 ROS_PYTHON_VERSION=3 ROS_LOCALHOST_ONLY=0 ROS_DISTRO=galactic
Problem:
Currently, I have a static tf2 transform between "map" and "planner" frame. The relationship is shown below.
$ ros2 run tf2_ros tf2_echo map planner
[INFO] [1688158439.517036056] [tf2_echo]: Waiting for transform map -> planner: Invalid frame ID "map" passed to canTransform argument target_frame - frame does not exist
At time 0.0
- Translation: [0.000, -1.000, 0.000]
- Rotation: in Quaternion [0.000, 0.000, -0.383, 0.924]
Basically, the "planner" frame is offset from the "map" frame by 1 meter in the y-axis and -45 degree in yaw ( all in ros2 unit standard).
However, when I tried to use the tf_buffer to transform (-0.645, -0.705) in "map" frame to "planner" frame, I got
x -0.6646803743153546 y -0.2474873734152917
, which is not the answer I expected. Originally, I was expecting both x should be less than 0.5 meters from the origin in "planner" frame.
Because base on the x-y graph below (I have transformed the point from ROS standard to normal cartesian coordinate), I was expecting the |x-value| of "planner" frame is < 0.5 meters. , but it turned out to be -0.66468
code for doing the transform
def on_timer(self):
# my_pose = Pose()
# my_pose.position.x = -0.745000
# my_pose.position.y = -0.705000
# my_pose.position.z = 0.0
# my_pose.orientation.x = 0.0
# my_pose.orientation.y = 0.0
# my_pose.orientation.z = 0.0
# my_pose.orientation.w = 1.0
# testPose = PoseStamped()
# testPose.pose = my_pose
# testPose.header.frame_id = "map"
# testPose.header.stamp = self.get_clock().now().to_msg()
# resultPose = self.tf_buffer.transform(PoseStamped, "planner", rclpy.time.Time() )
# self.get_logger().info(" x " + str(resultPose.pose.position.x) + "y " + str(resultPose.pose.position.y))
# Define the input pose in the source frame
source_pose = PoseStamped()
source_pose.header.frame_id = 'map'
source_pose.pose.position.x = -0.645
source_pose.pose.position.y = -0.705
source_pose.pose.position.z = 0.0
source_pose.pose.orientation.x = 0.0
source_pose.pose.orientation.y = 0.0
source_pose.pose.orientation.z = 0.0
source_pose.pose.orientation.w = 1.0
# q = quaternion_from_euler(0, 0, -45* (3.14/180))
# source_pose.pose.orientation.x = q[0]
# source_pose.pose.orientation.y = q[1]
# source_pose.pose.orientation.z = q[2]
# source_pose.pose.orientation.w = q[3]
# Transform the pose to the target frame
d = Duration()
try:
target_pose = self.tf_buffer.transform(source_pose, 'planner', d )
self.get_logger().info("Transformed pose: x "+ str( target_pose.pose.position.x) + " y " + str( target_pose.pose.position.y))
except tf2_ros.TransformException as e:
self.get_logger().error("Transform failed: %s", str(e))
Thus, I am wondering if this is a bug in the tf2? Or could I have misunderstood something?