calibration_transform_publisher_ptu.py

Go to the documentation of this file.

#!/usr/bin/python

import roslib; roslib.load_manifest('camera_pose_calibration')
import tf
import rospy
import sys
import PyKDL
import time
import threading
from tf_conversions import posemath
from geometry_msgs.msg import TransformStamped, Pose
from camera_pose_calibration.msg import CameraCalibration


class CameraPublisher:
    def __init__(self, pose, child_frame_id, parent_frame_id):
        self.lock = threading.Lock()
        self.pub = tf.TransformBroadcaster()
        self.set_pose(pose, child_frame_id, parent_frame_id)

    def set_pose(self, pose, child_frame_id, parent_frame_id):
        with self.lock:
            self.transform = TransformStamped()
            self.transform.header.frame_id = parent_frame_id
            self.transform.child_frame_id = child_frame_id
            self.transform.transform.translation.x = pose.position.x
            self.transform.transform.translation.y = pose.position.y
            self.transform.transform.translation.z = pose.position.z
            self.transform.transform.rotation.x = pose.orientation.x
            self.transform.transform.rotation.y = pose.orientation.y
            self.transform.transform.rotation.z = pose.orientation.z
            self.transform.transform.rotation.w = pose.orientation.w

    def publish(self):
        with self.lock:
            #print("publishing")
            self.transform.header.stamp = rospy.Time.now()# + rospy.Duration(0.5)
            #self.pub.sendTransform(self.transform)
            q = (self.transform.transform.rotation.x,self.transform.transform.rotation.y,self.transform.transform.rotation.z,self.transform.transform.rotation.w)
            self.pub.sendTransform((self.transform.transform.translation.x,self.transform.transform.translation.y,self.transform.transform.translation.z),q,self.transform.header.stamp,self.transform.child_frame_id,self.transform.header.frame_id)


class CalibrationPublishManager:
    def __init__(self):
        self.lock = threading.Lock()
        self.publish_list = {}
        self.sub = rospy.Subscriber('camera_calibration', CameraCalibration, self.cal_cb)
        self.count = 0
        self.tf = tf.TransformListener(True, rospy.Duration(15.0))

    def cal_cb(self, msg):
        with self.lock:
            success = 0
            while success<20:
                try:
                    t = rospy.Time.now()
                    #self.tf.waitForTransform("/camera_4", "/ptu_mounting", t, rospy.Duration(10.0))
                    arm_position, arm_orientation = self.tf.lookupTransform("/camera_4", "/ptu_mounting",rospy.Time(0))
                    success = 20
                except:
                    #print "Unexpected error:", sys.exc_info()[0]
                    time.sleep(0.3)
                    success = success+1
            print(arm_position)
            arm_pose = Pose()
            
            arm_pose.position.x, arm_pose.position.y, arm_pose.position.z = arm_position[0], arm_position[1], arm_position[2]
            arm_pose.orientation.x, arm_pose.orientation.y, arm_pose.orientation.z, arm_pose.orientation.w = arm_orientation[0], arm_orientation[1], arm_orientation[2], arm_orientation[3]
            arm = posemath.fromMsg(arm_pose);
            self.publish_list = {}
            i = 0
            camera0 = None;
            f0 = None;
            for pose, camera in zip(msg.camera_pose, msg.camera_id):
                i= i+1
                if i==1:
                    print(camera)
                    print(pose)
                    f0 = posemath.fromMsg(pose)
                    print(f0)
                    camera0 = camera
                    continue
                f = posemath.fromMsg(pose)
                f1 = f0.Inverse()*f
#                pose = posemath.toMsg(f1)
#                print("%s relative pose of to %s"%(camera,camera0))
#                print(pose)
#                self.publish_list[camera] = CameraPublisher(pose, camera, camera0)
                print("%s relative pose of to %s"%(camera,"/ptu_mounting"))
                f2 = arm.Inverse()*f1;
                pose = posemath.toMsg(f2)
                print(pose)
                self.publish_list[camera] = CameraPublisher(pose, camera, "/ptu_mounting")
            sys.stdout.write(" ")


    def publish(self):
        with self.lock:
            self.count = self.count+1
            psym = "-\\|/"
            sys.stdout.write("\b%c"%psym[self.count % 4])
            sys.stdout.flush()
            #print("\b%c"%psym[self.count % 4], end="")
            for name, pub in self.publish_list.iteritems():
                pub.publish()



def main():
    rospy.init_node('calibration_tf_publisher')
    r = rospy.Rate(20)
    c = CalibrationPublishManager()
    while not rospy.is_shutdown():
        c.publish()
        try:
            r.sleep()
        except:
            print "Shutting down"


if __name__ == '__main__':
    main()