TF2 transform returns exception.
I am running a code where i am getting a pose from a publisher and I subscribe to it in this code: the type of message is geometry_msgs.msg.PoseStamped. I then try to transform that from the camera frame to the base frame but the tf2 transform does not work. How do I fix this? I even tried to redifine the pose stamped inside the function to make sure there where no errors.
import rospy
from std_msgs.msg import String
import tf2_ros
import geometry_msgs.msg
def callback(pose: geometry_msgs.msg.PoseStamped):
print(type(pose.pose))
object_pose = geometry_msgs.msg.PoseStamped()
pose.header.stamp = rospy.Time.now()
object_pose.header.frame_id = "camera_link"
object_pose.pose.position.x = 0
object_pose.pose.position.y = 0
object_pose.pose.position.z = 0
object_pose.pose.orientation.x = 0
object_pose.pose.orientation.y = 0
object_pose.pose.orientation.z = 0
object_pose.pose.orientation.w = 1
try:
object_world_pose = tf_buffer.transform(object_pose,"base_link")
except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException):
pass
print(object_world_pose)
print(pose)
def listener():
rospy.Subscriber("chatter", geometry_msgs.msg.PoseStamped, callback)
rospy.spin()
if __name__ == '__main__':
# Initialize ROS node to transform object pose.
print("Initializing node ...")
rospy.init_node('transform_object_poses', anonymous=True)
# Create a TF buffer in the global scope
print("Creating tf buffer ...")
tf_buffer = tf2_ros.Buffer()
tf_listener = tf2_ros.TransformListener(tf_buffer)
listener()
The error message:
[ERROR] [1684417447.350180]: bad callback:
Asked by hamzaay on 2023-05-18 08:48:06 UTC
Answers
If you want to transform objects you need to include the implementation of their conversions. You need to import the right module from https://index.ros.org/p/tf2_geometry_msgs/
The implementation is here: with to and from message conversions and transform implementations: https://github.com/ros/geometry2/blob/noetic-devel/tf2_geometry_msgs/src/tf2_geometry_msgs/tf2_geometry_msgs.py
Asked by tfoote on 2023-05-30 19:55:56 UTC
Comments