skeliton joint angles from openni_tracker

edit

I'm trying to read human joint angles using tf messages posted by openni_tracker. Tracker posts tf messages and it seems these are normally read with a tf::TransformListener object. I got this part to work, and I am receiving the transforms, but I have a few problems. (Has anyone done this already? Is there a stack/package or code?)

1): The TransformListener.lookupTransform() method takes two strings for the frames to get the transform between. I've been using things like "/right_shoulder_1" to indicate the right shoulder of the first skeleton the tracker sees. The problem is that if the tracker looses the first person, all these frames are deleted and it starts using "/right_shoulder_2" for the next person. How can I just get the transforms for whatever skeleton is currently being tracked?

2) To get the angle between limbs, I've basically been trying to get the 3D vectors for each joint, compute the vectors between them representing limbs between two joints (taking the difference between joint vectors), and calculating the angle between these limb vectors. Doing this through the TransformListner seems very inefficient - I don;t need any information other than the relative vector positions of the joints. Is there a better way?

Thanks!

Here's the code I have so far for just printing the elbow angle. (Doesn't work, seems to always print the same angle of 1.546 (~88.6 degrees))

#include <ros/ros.h>
#include <actionlib/client/simple_action_client.h>
#include <arm_navigation_msgs/MoveArmAction.h>
#include "std_msgs/String.h"
#include <tf/transform_listener.h>

#define my_pi 3.141592653589793 // Define the PI number for vector calculations

void getTransform(tf::StampedTransform t, tf::TransformListener *l, const std::string &from, const std::string &to){
    try{
        (*l).lookupTransform(   from,           //from frame
                                to,     //to frame
                                ros::Time(0),       //latest
                                t);     //object to store result in
    }
    catch (tf::TransformException ex){
        ROS_ERROR("%s",ex.what());
    }
}

void printTransform(tf::StampedTransform t){
    //output angle
    printf("%lf", t.getOrigin().x());
    printf("\t");
    printf("%lf", t.getOrigin().y());
    printf("\t");
    printf("%lf", t.getOrigin().z());
    printf("\n");
}

int main(int argc, char **argv){

    ros::init(argc, argv, "kinect_control");

    ros::NodeHandle node;

    tf::TransformListener listener;

    double elbowAngle=0.0;

    //main loop
    ros::Rate rate(10.0);
    while (node.ok()){
        tf::StampedTransform rShoulderToRElbow;
        getTransform(rShoulderToRElbow,&listener,"/right_shoulder_2","/right_elbow_2");

        tf::StampedTransform rElbowToRHand;
        getTransform(rShoulderToRElbow,&listener,"/right_elbow_2","/right_hand_2");

        //math
        elbowAngle=(rShoulderToRElbow.getOrigin().angle(rElbowToRHand.getOrigin()));

        printf("%lf", elbowAngle);
        printf("\n");

        rate.sleep();
    }

    //ros::spin();

    return 0;
}