Robotics StackExchange | Archived questions

simultanuous transform for tf for multi tracking

Hi ,

I will so grateful if you help me, I want to extract all 3d joints for all users detected . But my code give me the last transformed frame for multi users (joints of one user)and I want to do simultaneous transform for multi users. Thank you for advance .

EDIT

Ok thank you for your time . this is my code can you please tell me where can I parse the transformations for all users .here if I have 2 users only righthip2 and other joints is transformed but righthip1 is not transformed . I hope it makes sense . This is my code:

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

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

    string MAIN_FRAME_camera = "openni_depth_frame"; string MAIN_FRAME = "torso_1"; string FRAMES[] = {"head_", "neck_", "torso_", "left_shoulder_", "right_shoulder_", "left_hand_", "right_hand_", "left_elbow_", "right_elbow_", "left_hip_", "right_hip_", "left_knee_", "right_knee_", "left_foot_", "right_foot_"};

    ros::NodeHandle node;

    tf::TransformListener listener;

    ros::Rate rate(30.0);

    ofstream myfile; ofstream myfile2;

    if( remove("/home/amal/catkin_ws/src/isr_activity_recognition/test_torso.txt") != 0 || remove("/home/amal/catkin_ws/src/isr_activity_recognition/test_camera.txt") != 0) perror( "Error deleting file" ); else puts( "File successfully deleted" ); char last_char;

    while (node.ok()){ int contador = 0;

        while(listener.frameExists("torso_1")==0 && listener.frameExists("torso_2")==0 && listener.frameExists("torso_3")==0 && listener.frameExists("torso_4")==0 && listener.frameExists("torso_5")==0 && listener.frameExists("torso_6")==0 && listener.frameExists("torso_7")==0){
            ros::Duration(0.1).sleep();
            ROS_INFO("Waiting..."); 
        }
        myfile.open("/home/amal/catkin_ws/src/isr_activity_recognition/test_torso.txt", ios::app);
        myfile2.open("/home/amal/catkin_ws/src/isr_activity_recognition/test_camera.txt", ios::app);  
        vector <string> ids;
        listener.getFrameStrings(ids);  

        //while(contador==0){
        //ros::Duration(0.5).sleep();
        //listener.getFrameStrings(ids);
        for(vector<string>::iterator i = ids.begin(); i < ids.end(); i++){
            string elem = *i;
            if (strstr(elem.c_str(),"torso_") != NULL){
                contador++;
                cout << "Numero de pessoas: " << contador <<endl;
                last_char = elem[elem.length()-1];

            }  

        }

        MAIN_FRAME = "torso_" + boost::lexical_cast<std::string>(int(last_char)-'0');
        cout << "MAIN_FRAME: " << MAIN_FRAME <<endl;

        //FRAMES[i] = FRAMES[i] + boost::lexical_cast<std::string>(contador);

        //ros::Duration(5).sleep();  
        tf::StampedTransform tf_head;
        tf::StampedTransform tf_neck;
        tf::StampedTransform tf_torso;
        tf::StampedTransform tf_left_shoulder;
        tf::StampedTransform tf_right_shoulder;
        tf::StampedTransform tf_left_hand;
        tf::StampedTransform tf_right_hand;
        tf::StampedTransform tf_left_elbow;
        tf::StampedTransform tf_right_elbow;
        tf::StampedTransform tf_left_hip;
        tf::StampedTransform tf_right_hip;
        tf::StampedTransform tf_left_knee;
        tf::StampedTransform tf_right_knee;
        tf::StampedTransform tf_left_foot;
        tf::StampedTransform tf_right_foot;

        try{
            ros::Duration timeout(1.0 / 30);
            listener.waitForTransform("torso_1", "openni_depth_frame", ros::Time(0), timeout );
            // head
            // torso
            listener.waitForTransform(MAIN_FRAME,FRAMES[0]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME, FRAMES[0]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_head);

            double x_head = tf_head.getOrigin().x();
            double y_head = tf_head.getOrigin().y();
            double z_head = tf_head.getOrigin().z();

            double roll_head, pitch_head, yaw_head ;
            tf_head.getBasis().getRPY(roll_head, pitch_head, yaw_head);
            tf::Quaternion q_head = tf_head.getRotation();

            // camera
            listener.waitForTransform(MAIN_FRAME_camera,FRAMES[0]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME_camera, FRAMES[0]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_head);

            double x_head_camera = tf_head.getOrigin().x();
            double y_head_camera = tf_head.getOrigin().y();
            double z_head_camera = tf_head.getOrigin().z();

            double roll_head_camera, pitch_head_camera, yaw_head_camera;
            tf_head.getBasis().getRPY(roll_head_camera, pitch_head_camera, yaw_head_camera);
            tf::Quaternion q_head_camera = tf_head.getRotation();

            //////////////////// NECK ////////////////////////// 
            // torso
            listener.waitForTransform(MAIN_FRAME,FRAMES[1]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME, FRAMES[1]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_neck);

            double x_neck = tf_neck.getOrigin().x();
            double y_neck = tf_neck.getOrigin().y();
            double z_neck = tf_neck.getOrigin().z();

            double roll_neck, pitch_neck, yaw_neck ;
            tf_neck.getBasis().getRPY(roll_neck, pitch_neck, yaw_neck);
            tf::Quaternion q_neck = tf_neck.getRotation();
            // camera
            listener.waitForTransform(MAIN_FRAME_camera,FRAMES[1]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME_camera, FRAMES[1]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_neck);

            double x_neck_camera = tf_neck.getOrigin().x();
            double y_neck_camera = tf_neck.getOrigin().y();
            double z_neck_camera = tf_neck.getOrigin().z();

            double roll_neck_camera, pitch_neck_camera, yaw_neck_camera;
            tf_neck.getBasis().getRPY(roll_neck_camera, pitch_neck_camera, yaw_neck_camera);
            tf::Quaternion q_neck_camera = tf_neck.getRotation();

            ////////////////// TORSO ////////////////////////////
            // torso
            listener.waitForTransform(MAIN_FRAME,FRAMES[2]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME, FRAMES[2]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_torso);

            double x_torso = tf_torso.getOrigin().x();
            double y_torso = tf_torso.getOrigin().y();
            double z_torso = tf_torso.getOrigin().z();

            double roll_torso, pitch_torso, yaw_torso ;
            tf_torso.getBasis().getRPY(roll_torso, pitch_torso, yaw_torso);
            tf::Quaternion q_torso = tf_torso.getRotation();

            // camera
            listener.waitForTransform(MAIN_FRAME_camera,FRAMES[2]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME_camera, FRAMES[2]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_torso);

            double x_torso_camera = tf_torso.getOrigin().x();
            double y_torso_camera = tf_torso.getOrigin().y();
            double z_torso_camera = tf_torso.getOrigin().z();

            double roll_torso_camera, pitch_torso_camera, yaw_torso_camera ;
            tf_torso.getBasis().getRPY(roll_torso_camera, pitch_torso_camera, yaw_torso_camera);
            tf::Quaternion q_torso_camera = tf_torso.getRotation();

            //////////////////// left shoulder ///////////////////////
            // torso
            listener.waitForTransform(MAIN_FRAME,FRAMES[3]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME, FRAMES[3]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_left_shoulder);

            double x_left_shoulder = tf_left_shoulder.getOrigin().x();
            double y_left_shoulder = tf_left_shoulder.getOrigin().y();
            double z_left_shoulder = tf_left_shoulder.getOrigin().z();

            double roll_left_shoulder, pitch_left_shoulder, yaw_left_shoulder ;
            tf_left_shoulder.getBasis().getRPY(roll_left_shoulder, pitch_left_shoulder, yaw_left_shoulder);
            tf::Quaternion q_left_shoulder = tf_left_shoulder.getRotation();

            // camera
            listener.waitForTransform(MAIN_FRAME_camera,FRAMES[3]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME_camera, FRAMES[3]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_left_shoulder);

            double x_left_shoulder_camera = tf_left_shoulder.getOrigin().x();
            double y_left_shoulder_camera = tf_left_shoulder.getOrigin().y();
            double z_left_shoulder_camera = tf_left_shoulder.getOrigin().z();

            double roll_left_shoulder_camera, pitch_left_shoulder_camera, yaw_left_shoulder_camera ;
            tf_left_shoulder.getBasis().getRPY(roll_left_shoulder_camera, pitch_left_shoulder_camera, yaw_left_shoulder_camera);
            tf::Quaternion q_left_shoulder_camera = tf_left_shoulder.getRotation();

            ////////////////////////// right shoulder ///////////////////////////
            // torso
            listener.waitForTransform(MAIN_FRAME,FRAMES[4]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME, FRAMES[4]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_right_shoulder);

            double x_right_shoulder = tf_right_shoulder.getOrigin().x();
            double y_right_shoulder = tf_right_shoulder.getOrigin().y();
            double z_right_shoulder = tf_right_shoulder.getOrigin().z();

            double roll_right_shoulder, pitch_right_shoulder, yaw_right_shoulder ;
            tf_right_shoulder.getBasis().getRPY(roll_right_shoulder, pitch_right_shoulder, yaw_right_shoulder);
            tf::Quaternion q_right_shoulder = tf_right_shoulder.getRotation();

            // camera
            listener.waitForTransform(MAIN_FRAME_camera,FRAMES[4]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME_camera, FRAMES[4]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_right_shoulder);

            double x_right_shoulder_camera = tf_right_shoulder.getOrigin().x();
            double y_right_shoulder_camera = tf_right_shoulder.getOrigin().y();
            double z_right_shoulder_camera = tf_right_shoulder.getOrigin().z();

            double roll_right_shoulder_camera, pitch_right_shoulder_camera, yaw_right_shoulder_camera ;
            tf_right_shoulder.getBasis().getRPY(roll_right_shoulder_camera, pitch_right_shoulder_camera, yaw_right_shoulder_camera);
            tf::Quaternion q_right_shoulder_camera = tf_right_shoulder.getRotation();

            /////////////////////////// left hand /////////////////////////////////
            // torso
            listener.waitForTransform(MAIN_FRAME,FRAMES[5]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME, FRAMES[5]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_left_hand);

            double x_left_hand = tf_left_hand.getOrigin().x();
            double y_left_hand = tf_left_hand.getOrigin().y();
            double z_left_hand = tf_left_hand.getOrigin().z();

            double roll_left_hand, pitch_left_hand, yaw_left_hand ;
            tf_left_hand.getBasis().getRPY(roll_left_hand, pitch_left_hand, yaw_left_hand);
            tf::Quaternion q_left_hand = tf_left_hand.getRotation();

            // camera
            listener.waitForTransform(MAIN_FRAME_camera,FRAMES[5]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME_camera, FRAMES[5]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_left_hand);

            double x_left_hand_camera = tf_left_hand.getOrigin().x();
            double y_left_hand_camera = tf_left_hand.getOrigin().y();
            double z_left_hand_camera = tf_left_hand.getOrigin().z();

            double roll_left_hand_camera, pitch_left_hand_camera, yaw_left_hand_camera ;
            tf_left_hand.getBasis().getRPY(roll_left_hand_camera, pitch_left_hand_camera, yaw_left_hand_camera);
            tf::Quaternion q_left_hand_camera = tf_left_hand.getRotation();

            ////////////////////////// right hand //////////////////////////
            // torso
            listener.waitForTransform(MAIN_FRAME,FRAMES[6]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME, FRAMES[6]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_right_hand);

            double x_right_hand = tf_right_hand.getOrigin().x();
            double y_right_hand = tf_right_hand.getOrigin().y();
            double z_right_hand = tf_right_hand.getOrigin().z();

            double roll_right_hand, pitch_right_hand, yaw_right_hand ;
            tf_right_hand.getBasis().getRPY(roll_right_hand, pitch_right_hand, yaw_right_hand);
            tf::Quaternion q_right_hand = tf_right_hand.getRotation();

            // camera
            listener.waitForTransform(MAIN_FRAME_camera,FRAMES[6]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME_camera, FRAMES[6]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_right_hand);

            double x_right_hand_camera = tf_right_hand.getOrigin().x();
            double y_right_hand_camera = tf_right_hand.getOrigin().y();
            double z_right_hand_camera = tf_right_hand.getOrigin().z();

            double roll_right_hand_camera, pitch_right_hand_camera, yaw_right_hand_camera ;
            tf_right_hand.getBasis().getRPY(roll_right_hand_camera, pitch_right_hand_camera, yaw_right_hand_camera);
            tf::Quaternion q_right_hand_camera = tf_right_hand.getRotation();

            /////////////////////////// left elbow /////////////////////////
            // torso
            listener.waitForTransform(MAIN_FRAME,FRAMES[7]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME, FRAMES[7]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_left_elbow);

            double x_left_elbow = tf_left_elbow.getOrigin().x();
            double y_left_elbow = tf_left_elbow.getOrigin().y();
            double z_left_elbow = tf_left_elbow.getOrigin().z();

            double roll_left_elbow, pitch_left_elbow, yaw_left_elbow ;
            tf_left_elbow.getBasis().getRPY(roll_left_elbow, pitch_left_elbow, yaw_left_elbow);
            tf::Quaternion q_left_elbow = tf_left_elbow.getRotation();

            // camera
            listener.waitForTransform(MAIN_FRAME_camera,FRAMES[7]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME_camera, FRAMES[7]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_left_elbow);

            double x_left_elbow_camera = tf_left_elbow.getOrigin().x();
            double y_left_elbow_camera = tf_left_elbow.getOrigin().y();
            double z_left_elbow_camera = tf_left_elbow.getOrigin().z();

            double roll_left_elbow_camera, pitch_left_elbow_camera, yaw_left_elbow_camera ;
            tf_left_elbow.getBasis().getRPY(roll_left_elbow_camera, pitch_left_elbow_camera, yaw_left_elbow_camera);
            tf::Quaternion q_left_elbow_camera = tf_left_elbow.getRotation();

            ////////////////////////// right elbow //////////////////////////////
            // torso
            listener.waitForTransform(MAIN_FRAME,FRAMES[8]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME, FRAMES[8]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_right_elbow);

            double x_right_elbow = tf_right_elbow.getOrigin().x();
            double y_right_elbow = tf_right_elbow.getOrigin().y();
            double z_right_elbow = tf_right_elbow.getOrigin().z();

            double roll_right_elbow, pitch_right_elbow, yaw_right_elbow ;
            tf_right_elbow.getBasis().getRPY(roll_right_elbow, pitch_right_elbow, yaw_right_elbow);
            tf::Quaternion q_right_elbow = tf_right_elbow.getRotation();

            // camera
            listener.waitForTransform(MAIN_FRAME_camera,FRAMES[8]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME_camera, FRAMES[8]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_right_elbow);

            double x_right_elbow_camera = tf_right_elbow.getOrigin().x();
            double y_right_elbow_camera = tf_right_elbow.getOrigin().y();
            double z_right_elbow_camera = tf_right_elbow.getOrigin().z();

            double roll_right_elbow_camera, pitch_right_elbow_camera, yaw_right_elbow_camera ;
            tf_right_elbow.getBasis().getRPY(roll_right_elbow_camera, pitch_right_elbow_camera, yaw_right_elbow_camera);
            tf::Quaternion q_right_elbow_camera = tf_right_elbow.getRotation();

            /////////////////////////// left hip ///////////////////////////
            // torso
            listener.waitForTransform(MAIN_FRAME,FRAMES[9]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME, FRAMES[9]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_left_hip);

            double x_left_hip = tf_left_hip.getOrigin().x();
            double y_left_hip = tf_left_hip.getOrigin().y();
            double z_left_hip = tf_left_hip.getOrigin().z();

            double roll_left_hip, pitch_left_hip, yaw_left_hip ;
            tf_left_hip.getBasis().getRPY(roll_left_hip, pitch_left_hip, yaw_left_hip);
            tf::Quaternion q_left_hip = tf_left_hip.getRotation();

            // camera
            listener.waitForTransform(MAIN_FRAME_camera,FRAMES[9]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME_camera, FRAMES[9]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_left_hip);

            double x_left_hip_camera = tf_left_hip.getOrigin().x();
            double y_left_hip_camera = tf_left_hip.getOrigin().y();
            double z_left_hip_camera = tf_left_hip.getOrigin().z();

            double roll_left_hip_camera, pitch_left_hip_camera, yaw_left_hip_camera ;
            tf_left_hip.getBasis().getRPY(roll_left_hip_camera, pitch_left_hip_camera, yaw_left_hip_camera);
            tf::Quaternion q_left_hip_camera = tf_left_hip.getRotation();

            ////////////////////////// right hip //////////////////////////////
            // torso
            listener.waitForTransform(MAIN_FRAME,FRAMES[10]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME, FRAMES[10]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_right_hip);

            double x_right_hip = tf_right_hip.getOrigin().x();
            double y_right_hip = tf_right_hip.getOrigin().y();
            double z_right_hip = tf_right_hip.getOrigin().z();

            double roll_right_hip, pitch_right_hip, yaw_right_hip ;
            tf_right_hip.getBasis().getRPY(roll_right_hip, pitch_right_hip, yaw_right_hip);
            tf::Quaternion q_right_hip = tf_right_hip.getRotation();

            // camera
            listener.waitForTransform(MAIN_FRAME_camera,FRAMES[10]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME_camera, FRAMES[10]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_right_hip);

            double x_right_hip_camera = tf_right_hip.getOrigin().x();
            double y_right_hip_camera = tf_right_hip.getOrigin().y();
            double z_right_hip_camera = tf_right_hip.getOrigin().z();

            double roll_right_hip_camera, pitch_right_hip_camera, yaw_right_hip_camera ;
            tf_right_hip.getBasis().getRPY(roll_right_hip_camera, pitch_right_hip_camera, yaw_right_hip_camera);
            tf::Quaternion q_right_hip_camera = tf_right_hip.getRotation();

            ///////////////////////////// left knee ///////////////////////////
            // torso
            listener.waitForTransform(MAIN_FRAME,FRAMES[11]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME, FRAMES[11]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_left_knee);

            double x_left_knee = tf_left_knee.getOrigin().x();
            double y_left_knee = tf_left_knee.getOrigin().y();
            double z_left_knee = tf_left_knee.getOrigin().z();

            double roll_left_knee, pitch_left_knee, yaw_left_knee ;
            tf_left_knee.getBasis().getRPY(roll_left_knee, pitch_left_knee, yaw_left_knee);
            tf::Quaternion q_left_knee = tf_left_knee.getRotation();

            // camera
            listener.waitForTransform(MAIN_FRAME_camera,FRAMES[11]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME_camera, FRAMES[11]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_left_knee);

            double x_left_knee_camera = tf_left_knee.getOrigin().x();
            double y_left_knee_camera = tf_left_knee.getOrigin().y();
            double z_left_knee_camera = tf_left_knee.getOrigin().z();

            double roll_left_knee_camera, pitch_left_knee_camera, yaw_left_knee_camera ;
            tf_left_knee.getBasis().getRPY(roll_left_knee_camera, pitch_left_knee_camera, yaw_left_knee_camera);
            tf::Quaternion q_left_knee_camera = tf_left_knee.getRotation();

            /////////////////////////// right knee ///////////////////////////////
            // torso
            listener.waitForTransform(MAIN_FRAME,FRAMES[12]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME, FRAMES[12]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_right_knee);

            double x_right_knee = tf_right_knee.getOrigin().x();
            double y_right_knee = tf_right_knee.getOrigin().y();
            double z_right_knee = tf_right_knee.getOrigin().z();

            double roll_right_knee, pitch_right_knee, yaw_right_knee ;
            tf_right_knee.getBasis().getRPY(roll_right_knee, pitch_right_knee, yaw_right_knee);
            tf::Quaternion q_right_knee = tf_right_knee.getRotation();

            // camera
            listener.waitForTransform(MAIN_FRAME_camera,FRAMES[12]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME_camera, FRAMES[12]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_right_knee);

            double x_right_knee_camera = tf_right_knee.getOrigin().x();
            double y_right_knee_camera = tf_right_knee.getOrigin().y();
            double z_right_knee_camera = tf_right_knee.getOrigin().z();

            double roll_right_knee_camera, pitch_right_knee_camera, yaw_right_knee_camera ;
            tf_right_knee.getBasis().getRPY(roll_right_knee_camera, pitch_right_knee_camera, yaw_right_knee_camera);
            tf::Quaternion q_right_knee_camera = tf_right_knee.getRotation();

            ////////////////////// left foot //////////////////////////
            // torso
            listener.waitForTransform(MAIN_FRAME,FRAMES[13]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME, FRAMES[13]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_left_foot);

            double x_left_foot = tf_left_foot.getOrigin().x();
            double y_left_foot = tf_left_foot.getOrigin().y();
            double z_left_foot = tf_left_foot.getOrigin().z();

            double roll_left_foot, pitch_left_foot, yaw_left_foot ;
            tf_left_foot.getBasis().getRPY(roll_left_foot, pitch_left_foot, yaw_left_foot);
            tf::Quaternion q_left_foot = tf_left_foot.getRotation();

            // camera
            listener.waitForTransform(MAIN_FRAME_camera,FRAMES[13]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME_camera, FRAMES[13]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_left_foot);

            double x_left_foot_camera = tf_left_foot.getOrigin().x();
            double y_left_foot_camera = tf_left_foot.getOrigin().y();
            double z_left_foot_camera = tf_left_foot.getOrigin().z();

            double roll_left_foot_camera, pitch_left_foot_camera, yaw_left_foot_camera ;
            tf_left_foot.getBasis().getRPY(roll_left_foot_camera, pitch_left_foot_camera, yaw_left_foot_camera);
            tf::Quaternion q_left_foot_camera = tf_left_foot.getRotation();

            ///////////////////////////// right foot ////////////////////////
            // torso
            listener.waitForTransform(MAIN_FRAME,FRAMES[14]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME, FRAMES[14]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_right_foot);

            double x_right_foot = tf_right_foot.getOrigin().x();
            double y_right_foot = tf_right_foot.getOrigin().y();
            double z_right_foot = tf_right_foot.getOrigin().z();

            double roll_right_foot, pitch_right_foot, yaw_right_foot ;
            tf_right_foot.getBasis().getRPY(roll_right_foot, pitch_right_foot, yaw_right_foot);
            tf::Quaternion q_right_foot = tf_right_foot.getRotation();

            // camera
            listener.waitForTransform(MAIN_FRAME_camera,FRAMES[14]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), timeout );
            listener.lookupTransform(MAIN_FRAME_camera, FRAMES[14]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), tf_right_foot);

            double x_right_foot_camera = tf_right_foot.getOrigin().x();
            double y_right_foot_camera = tf_right_foot.getOrigin().y();
            double z_right_foot_camera = tf_right_foot.getOrigin().z();

            double roll_right_foot_camera, pitch_right_foot_camera, yaw_right_foot_camera ;
            tf_right_foot.getBasis().getRPY(roll_right_foot_camera, pitch_right_foot_camera, yaw_right_foot_camera);
            tf::Quaternion q_right_foot_camera = tf_right_foot.getRotation();
        }
    }
}

Asked by amal1 on 2018-07-19 14:15:59 UTC

Comments

When running openni_tracker the /tf data to all joints for all tracked users is already provided. E.g., for user 1, you'd get a transform from openni_depth_frame to right_hip_1. By parsing all of these transforms, it sounds like you'd have exactly what you are asking for.

Asked by jarvisschultz on 2018-07-19 16:34:14 UTC

If you are looking for something else, I'd suggest re-phrasing your question to be more specific about what you need.

Asked by jarvisschultz on 2018-07-19 16:35:22 UTC

please take a look at the answer bellow because there is my code thank you Mr jarvisschultz.

Asked by amal1 on 2018-07-20 11:59:06 UTC

Hi @amal1 you should not post answers that are not actually answers. If you need to add information, you should edit your original question. Also, when posting code snippets, please use the 101010 button to properly format your code. The way you posted it is unreadable. I fixed it for you this time.

Asked by jarvisschultz on 2018-07-20 18:54:05 UTC

You are unlikely to get anywhere posting enormous code snippets. For someone to help you they now must parse a huge amount of code. My advice is to try to boil your question down to one specific question, and try to come up with a simpler example demonstrating what is not working.

Asked by jarvisschultz on 2018-07-20 18:55:20 UTC

ok . I am sorry , I am new to this forum . I meant by posting code to be more specific I showed an example bellow . for example if I have 2 users to track my code only transform head_2 ,neck_2 etc.. and it doesn't transform the joints of the first user . Does this makes sense ?

Asked by amal1 on 2018-07-21 03:48:07 UTC

Hi, my question is can I transform all frames tracked without losing users while new user is tracke, for example if there is frame head_1 is detected and then new user come my code only transform head_2 and forget the first even if the user is not lost . Can I transform both of the frames .Thank you

Asked by amal1 on 2018-07-23 06:45:38 UTC

I'm closing this question because you opened another question that is slightly better described here. If you think this should be reopened, please describe your reasoning.

Asked by jarvisschultz on 2018-07-23 14:20:37 UTC

Answers