tf_rgbOptical_base.py

Go to the documentation of this file.

#!/usr/bin/env python  
import roslib
roslib.load_manifest('ar_kinect')
from geometry_msgs.msg import Pose2D, PoseStamped, Pose
from ar_pose.msg import ARMarkers
from tf import TransformListener
from math import cos, sin, pi

import rospy
import tf
from std_msgs.msg import Float64

data = ARMarkers()
angle = 0.0

def callback(data):
    global angle
    angle = data.data
    #print angle
    
#def localize(markers):
    #global data
    #data = markers
    #global angle
 
if __name__ == '__main__':
    #print angle
    rospy.init_node('tf_rgbOptical_base')
    #Aktuellen Winkel der Kinect holen
    #Beim Winkel ist sowohl der Winkel des Motors wie auch die Schraeglage selbst der Kinect miteinberechnet
    #Um diesen Winkel wird die x-Achse gedreht
    rospy.Subscriber('cur_tilt_angle', Float64, callback)
    #Koordinaten zu den Markern holen
    #rospy.Subscriber('ar_pose_markers', ARMarkers, localize)
    
    #Winkel vorgeben fuer Kinect
    pub2 = rospy.Publisher('tilt_angle', Float64)
    

    
    #ARMarkers msg erzeugen
    br = tf.TransformBroadcaster()
 
    rate = rospy.Rate(10.0)
    
    while not rospy.is_shutdown():
        #Berechnen der Translation
        #l1 = 7.5cm l2 = 2.5cm l3 = 3cm
        pub2.publish(-52.0)
        stageAngle = 45
        x =  0.035 - 0.002315 + 0.025*sin(-52*pi/180)
        y =  0
        z =  0.0062 + 0.0932 + 0.02315 + 0.025*cos(-52*pi/180)
        #y = -0.075*cos(stageAngle) -0.025*cos(angle) + 0.03*sin(angle)
        #z = 0.075*cos(stageAngle) +0.025*cos(angle) + 0.03*sin(angle)
        now = rospy.Time.now()
        br.sendTransform((x, y, z), tf.transformations.quaternion_from_euler(0, 52*pi/180, 0),
                         now, "camera_link","gaze")     
  
        rate.sleep()