In [1]:

    

import tf
import rospy
import json
import rospkg
import transformations


    

In [17]:

    

def frame_publisher(Cskeleton, sensor, Kskeleton=None):
    tfb = tf.TransformBroadcaster()
    rospy.sleep(1)
    rate = rospy.Rate(10)
    while not rospy.is_shutdown():
        for key in Cskeleton.keys():
            if Kskeleton is not None:
                tfb.sendTransform(Kskeleton[key][0], Kskeleton[key][1], rospy.Time.now(),
                                  key+'_init', '/optitrack_frame')    
            tfb.sendTransform(Cskeleton[key][0], Cskeleton[key][1], rospy.Time.now(),
                              key, '/optitrack_frame')
            rate.sleep()


    

In [18]:

    

rospy.init_node('test_calibration')


    

In [19]:

    

rospack = rospkg.RosPack()


    

In [20]:

    

sensor = 'opt'


    

In [ ]:

    

sensor = 'kinect'


    

In [27]:

    

skeleton_file = rospack.get_path("human_moveit_config")+'/tmp/skeleton.json'
# read the T pose from human
with open(skeleton_file) as data_file:
    data_skeletons = json.load(data_file)
skeleton = data_skeletons[sensor]


    

In [28]:

    

calibration_file = rospack.get_path("human_moveit_config")+'/tmp/sensor_calibration.json'
# read the T pose from human
with open(calibration_file) as data_file:
    data_calibration = json.load(data_file)
calibration = data_calibration[sensor]


    

In [29]:

    

KtB = skeleton['human/base']


    

In [30]:

    

# convert back all skeletons frames into kinect
Kskeleton = {}
for key, value in skeleton.iteritems():
    if key != 'human/base':
        Kskeleton[key] = transformations.multiply_transform(KtB, value)
    else:
        Kskeleton[key] = value


    

In [31]:

    

# convert all skeleton data to their calibrated values
Cskeleton = {}
base_transform = transformations.inverse_transform(calibration['human/base'])
for key, value in calibration.iteritems():
    Cskeleton[key] = transformations.multiply_transform(Kskeleton[key], value)


    

In [12]:

    

frame_publisher(Cskeleton, sensor)


    

    




---------------------------------------------------------------------------
KeyboardInterrupt                         Traceback (most recent call last)
<ipython-input-12-f2fcf27b03b7> in <module>()
----> 1 frame_publisher(Cskeleton, sensor)

<ipython-input-2-6213e5fcafe0> in frame_publisher(Cskeleton, sensor, Kskeleton)
     10             tfb.sendTransform(Cskeleton[key][0], Cskeleton[key][1], rospy.Time.now(),
     11                               '/human/'+key+'_calibr', '/optitrack_frame')
---> 12             rate.sleep()

/opt/ros/indigo/lib/python2.7/dist-packages/rospy/timer.pyc in sleep(self)
     97         """
     98         curr_time = rospy.rostime.get_rostime()
---> 99         sleep(self._remaining(curr_time))
    100         self.last_time = self.last_time + self.sleep_dur
    101 

/opt/ros/indigo/lib/python2.7/dist-packages/rospy/timer.pyc in sleep(duration)
    123             return
    124         else:
--> 125             rospy.rostime.wallsleep(duration)
    126     else:
    127         initial_rostime = rospy.rostime.get_rostime()

/opt/ros/indigo/lib/python2.7/dist-packages/rospy/rostime.pyc in wallsleep(duration)
    294             pass
    295     else:
--> 296         time.sleep(duration)

KeyboardInterrupt: 

In [ ]:

    

frame_publisher(Cskeleton, sensor, Kskeleton)


    

In [ ]: