From a2c819d68e7594f4bdcef00c1174f033b4ad4817 Mon Sep 17 00:00:00 2001 From: shalenikol Date: Mon, 11 Dec 2023 18:17:34 +0300 Subject: [PATCH] get camera pose via TransformListener --- .../scripts/pose_estimation_lifecycle.py | 52 ++++++++++++------- 1 file changed, 34 insertions(+), 18 deletions(-) diff --git a/rbs_perception/scripts/pose_estimation_lifecycle.py b/rbs_perception/scripts/pose_estimation_lifecycle.py index d735185..4f504d6 100755 --- a/rbs_perception/scripts/pose_estimation_lifecycle.py +++ b/rbs_perception/scripts/pose_estimation_lifecycle.py @@ -23,15 +23,15 @@ from rclpy.lifecycle import Publisher from rclpy.lifecycle import State from rclpy.lifecycle import TransitionCallbackReturn from rclpy.timer import Timer -from rclpy.impl.logging_severity import LoggingSeverity +#from rclpy.impl.logging_severity import LoggingSeverity #from tf.transformations import quaternion_from_euler from ament_index_python.packages import get_package_share_directory from sensor_msgs.msg import Image, CameraInfo from geometry_msgs.msg import Pose, TransformStamped from tf2_ros import TransformBroadcaster, TransformException -#from tf2_ros.buffer import Buffer -#from tf2_ros.transform_listener import TransformListener +from tf2_ros.buffer import Buffer +from tf2_ros.transform_listener import TransformListener from cv_bridge import CvBridge # Package to convert between ROS and OpenCV Images import cv2 # OpenCV library @@ -89,7 +89,7 @@ class PoseEstimator(Node): self.publishDelay = 2.0 self.topicSrv = self.nodeName + "/detect6Dpose" self._InitService() - self.camera_pose = self.get_camera_pose() + self.camera_pose = Pose() #self.get_camera_pose() self.tmpdir = tempfile.gettempdir() self.mytemppath = Path(self.tmpdir) / "rbs_per" @@ -101,6 +101,8 @@ class PoseEstimator(Node): super().__init__(self.nodeName, **kwargs) self.declare_parameter("mesh_path", rclpy.Parameter.Type.STRING) + self.tf_buffer = Buffer() + self.tf_listener = TransformListener(self.tf_buffer, self) # Initialize the transform broadcaster self.tf_broadcaster = TransformBroadcaster(self) # Used to convert between ROS and OpenCV images @@ -110,20 +112,20 @@ class PoseEstimator(Node): self.objMeshFile = "" self.objPath = "" - def get_camera_pose(self) -> Pose: - # 3.3 2.8 2.8 0 0.5 -2.4 - p = Pose() - p.position.x = -2. - p.position.y = -0.55 - p.position.z = 1.44 - #R = rpyToMatrix([0, 0.5, -2.4]) - #q = pin.Quaternion() - #q = t3d.euler.euler2quat(0., 0.5, -2.4) - p.orientation.w = 0.9524 #q[0] - p.orientation.x = -0.0476 #q[1] - p.orientation.y = 0.213 #q[2] - p.orientation.z = 0.213 #q[3] - return p + # def get_camera_pose(self) -> Pose: + # # 3.3 2.8 2.8 0 0.5 -2.4 + # p = Pose() + # p.position.x = -2. + # p.position.y = -0.55 + # p.position.z = 1.44 + # #R = rpyToMatrix([0, 0.5, -2.4]) + # #q = pin.Quaternion() + # #q = t3d.euler.euler2quat(0., 0.5, -2.4) + # p.orientation.w = 0.9524 #q[0] + # p.orientation.x = -0.0476 #q[1] + # p.orientation.y = 0.213 #q[2] + # p.orientation.z = 0.213 #q[3] + # return p def publish(self): """Publish a new message when enabled.""" @@ -429,6 +431,20 @@ class PoseEstimator(Node): if not self._is_camerainfo: self.get_logger().warning("No data from CameraInfo") return + # get camera pose + camera_name = self.topicImage.split('/')[1] + try: + t = self.tf_buffer.lookup_transform("world", camera_name, rclpy.time.Time()) + except TransformException as ex: + self.get_logger().info(f'Could not transform {camera_name} to world: {ex}') + return + self.camera_pose.position.x = t.transform.translation.x + self.camera_pose.position.y = t.transform.translation.y + self.camera_pose.position.z = t.transform.translation.z + self.camera_pose.orientation.w = t.transform.rotation.w + self.camera_pose.orientation.x = t.transform.rotation.x + self.camera_pose.orientation.y = t.transform.rotation.y + self.camera_pose.orientation.z = t.transform.rotation.z # Convert ROS Image message to OpenCV image current_frame = self.br.imgmsg_to_cv2(data)