robossembler/runtime
7
0
Fork 0
Code Issues 4 Pull requests Projects Releases Packages Wiki Activity Actions

get camera pose via TransformListener

Browse source
This commit is contained in:
shalenikol 2023-12-11 18:17:34 +03:00
parent 2db4d1eb67
commit a2c819d68e
1 changed files with 34 additions and 18 deletions
Download patch file Download diff file Expand all files Collapse all files

52
rbs_perception/scripts/pose_estimation_lifecycle.py
View file

@ -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:
# <camera_pose>3.3 2.8 2.8 0 0.5 -2.4</camera_pose>
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:
# # <camera_pose>3.3 2.8 2.8 0 0.5 -2.4</camera_pose>
# 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)