tf message filter with cob_perception_msgs::ColorDepthImageArray

I have a project for activities analysis and face recognition . I just link my project of activity recognition which is subscribed to tf topic and the project of "cob_people_perception" but the problem is there no synchronization to link every skeleton with it's appropriate face. I will be so grateful if you help me how to synchronize tf topic with cob_perception_msgs::ColorDepthImageArray topic with tf:MessageFilter . Bellow is the code where I want to make synchronization .Thank you for advance . I try with this code in the file face_recognizer_node.cpp in main function but it shows unkown error . Platform: elementary Os Ros version ::kinetic Packages: Openni tracker , cob perception detection int main(int argc, char** argv) { // Initialize ROS, specify name of node ros::init(argc, argv, "face_recognizer"); // Create a handle for this node, initialize node ros::NodeHandle nh("~"); face_recognition_publisher_ = nh.advertise<:detectionarray>("face_recognitions", 1); tf::MessageFilter<:colordepthimagearray> * tf_filter_; message_filters::Subscriber<:colordepthimagearray> cloud2_sub_; cloud2_sub_.subscribe(nh,"face_positions", 10); //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; boost::shared_ptr p; //FRAMES[i] = FRAMES[i] + boost::lexical_cast<:string>(contador); tf_filter_ = new tf::MessageFilter<:colordepthimagearray>(cloud2_sub_, listener ,"openni_depth_frame", 10); tf_filter_->registerCallback( boost::bind(&FaceRecognizerNode::facePositionsCallback,p,_1) ); 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 ids; listener.getFrameStrings(ids); //while(contador==0){ //ros::Duration(0.5).sleep(); //listener.getFrameStrings(ids); for(vector::iterator i = ids.begin(); i (int(last_char)-'0'); cout (int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME, FRAMES[0]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME_camera, FRAMES[0]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME, FRAMES[1]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME_camera, FRAMES[1]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME, FRAMES[2]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME_camera, FRAMES[2]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME, FRAMES[3]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME_camera, FRAMES[3]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME, FRAMES[4]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME_camera, FRAMES[4]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME, FRAMES[5]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME_camera, FRAMES[5]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME, FRAMES[6]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME_camera, FRAMES[6]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME, FRAMES[7]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME_camera, FRAMES[7]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME, FRAMES[8]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME_camera, FRAMES[8]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME, FRAMES[9]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME_camera, FRAMES[9]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME, FRAMES[10]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME_camera, FRAMES[10]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME, FRAMES[11]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME_camera, FRAMES[11]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME, FRAMES[12]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME_camera, FRAMES[12]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME, FRAMES[13]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME_camera, FRAMES[13]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME, FRAMES[14]+ boost::lexical_cast<: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<:string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0)); listener.lookupTransform(MAIN_FRAME_camera, FRAMES[14]+ boost::lexical_cast<: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(); // End of body transforms // file em relacao ao torso if (myfile.is_open()){ myfile