simultanuous transform for tf for multi tracking [closed]

edit

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 .

• Platform : ubuntu
• ROS: kinetic
• Package : openni_tracker

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 right_hip_2 and other joints is transformed but right_hip_1 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 ...