ROS Resources: Documentation | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange
Ask Your Question

Revision history [back]

click to hide/show revision 1
initial version

Thank you for your help . But the solution is very simple due to lack of concentration . I just close the loop that receive all frames transformed at the end of code . In this way tf transform transform frames tracked one by one .

     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" );
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);
vector <string> ids;
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;

    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();  }  }