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<cob_perception_msgs::DetectionArray>("face_recognitions", 1);
tf::MessageFilter<cob_perception_msgs::ColorDepthImageArray> * tf_filter_;
message_filters::Subscriber<cob_perception_msgs::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<facerecognizernode> p; //FRAMES[i] = FRAMES[i] + boost::lexical_cast<std::string>(contador); tf_filter_ = new tf::MessageFilter<cob_perception_msgs::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 <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;
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{
listener.waitForTransform(MAIN_FRAME, FRAMES[0]+ boost::lexical_cast<std::string>(int(last_char)-'0'), ros::Time(0), ros::Duration(4.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 ...