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("face_recognitions", 1);
tf::MessageFilter * tf_filter_;
message_filters::Subscriber 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(contador);
tf_filter_ = new tf::MessageFilter(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 < ids.end(); i++){
string elem = *i;
if (strstr(elem.c_str(),"torso_") != NULL){
contador++;
cout << "Numero de pessoas: " << contador <(int(last_char)-'0');
cout << "MAIN_FRAME: " << MAIN_FRAME <(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME, FRAMES[0]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME_camera, FRAMES[0]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME, FRAMES[1]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME_camera, FRAMES[1]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME, FRAMES[2]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME_camera, FRAMES[2]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME, FRAMES[3]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME_camera, FRAMES[3]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME, FRAMES[4]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME_camera, FRAMES[4]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME, FRAMES[5]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME_camera, FRAMES[5]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME, FRAMES[6]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME_camera, FRAMES[6]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME, FRAMES[7]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME_camera, FRAMES[7]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME, FRAMES[8]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME_camera, FRAMES[8]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME, FRAMES[9]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME_camera, FRAMES[9]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME, FRAMES[10]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME_camera, FRAMES[10]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME, FRAMES[11]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME_camera, FRAMES[11]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME, FRAMES[12]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME_camera, FRAMES[12]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME, FRAMES[13]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME_camera, FRAMES[13]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME, FRAMES[14]+ boost::lexical_cast(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(int(last_char)-'0'), ros::Time(0), ros::Duration(4.0));
listener.lookupTransform(MAIN_FRAME_camera, FRAMES[14]+ boost::lexical_cast(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 << ros::Time::now()<<" " ;
myfile << x_head<<" "<
↧