tf listener with multi users tracked t the same time associate an id with each user tracked

asked 2018-07-10 04:39:04 -0500

amal gravatar image

Hi, I found a tflistener.cpp code , and it give me in file 3d body joints for user tracked by openni-tracker , my question is how can I enable multi users in tflistener.cpp, and can I associate for every skeleton an ID to distinguish between users . Thank you for advance . Plateform : Elementary OS Ros version : Kinetic

    // Initialize ROS, specify name of node
ros::init(argc, argv, "face_recognizer");

// Create a handle for this node, initialize node
ros::NodeHandle nh("~");

// Create FaceRecognizerNode class instance
FaceRecognizerNode face_recognizer_node(nh);
    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_"};

tf::TransformListener listener;

ros::Rate rate(5.0);

ofstream myfile; ofstream myfile2;

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

while (nh.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/cob_people_perception/cob_people_detection/files/test_torso.txt", ios::app);
myfile2.open("/home/amal/catkin_ws/src/cob_people_perception/cob_people_detection/files/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);
//for (int i ; i<contador; i++){
//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 / 5);
       //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), ros::Duration(1.0));
    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), ros::Duration(1.0));
    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 ...
(more)
edit retag flag offensive close merge delete

Comments

hi , Any news please ?

amal gravatar imageamal ( 2018-07-10 12:39:50 -0500 )edit