int main(int argc, char** argv) { ros::init(argc, argv, "follow_me"); ros::NodeHandle nh; std::string user_to_track; std::string frame_id("camera_depth_frame"); nh.getParam("camera_frame_id", frame_id); int skeleton_to_track = 0; ros::param::set("skeleton_to_track", skeleton_to_track); ROS_INFO("Waiting for user identity."); while(!ros::param::get("user_to_track", user_to_track) && nh.ok()) { ros::Duration(1).sleep(); } tf::TransformListener listener; ros::Publisher pub = nh.advertise<geometry_msgs::Twist>("/kobra/cmd_vel", 1); while(nh.ok()) { ROS_INFO("Looking for %s's face.", user_to_track.c_str()); std::string skeleton_to_track_frame; while(nh.ok()) //Search continuously for a skeleton head very close to the designated user face. { std::vector<tf::StampedTransform> transforms; lookForEveryHeadTransform(listener, transforms, user_to_track); if(findClosestHeadToFace(transforms, skeleton_to_track_frame)) { skeleton_to_track = changeFrameAndReturnIndex(skeleton_to_track_frame); ros::param::set("skeleton_to_track", skeleton_to_track); break; } geometry_msgs::Twist twist; twist.linear.x = twist.angular.z = 0; pub.publish(twist); ros::Rate(30).sleep(); } ROS_INFO("User %s and skeleton %s associated. Start tracking.", user_to_track.c_str(), skeleton_to_track_frame.c_str()); while(ros::param::get("skeleton_to_track", skeleton_to_track) && nh.ok()) { if(skeleton_to_track == 0) { ROS_INFO("User %s and skeleton %s association lost. Stop tracking.", user_to_track.c_str(), skeleton_to_track_frame.c_str()); break; } tf::StampedTransform transform; if(lookForSpecificBodyTransform(listener, frame_id, skeleton_to_track_frame, transform)) { double distance = std::sqrt(std::pow(transform.getOrigin().getX(), 2) + std::pow(transform.getOrigin().getY(), 2) + std::pow(transform.getOrigin().getZ(), 2)); //TODO Ho la distanza, in base ad essa restituisco la percentuale di velocità del robot. geometry_msgs::Twist twist; if(distance < 1.35) { //Mi allontano twist.linear.x = -0.4; } else if(distance > 1.65) { //Mi avvicino twist.linear.x = 0.4; } if(transform.getOrigin().getY() > 0.2) { //Giro a sinistra twist.angular.z = 0.5; } else if(transform.getOrigin().getY() < -0.2) { //Giro a destra twist.angular.z = -0.5; } pub.publish(twist); } ros::Rate(30).sleep(); } } ros::shutdown(); return EXIT_SUCCESS; }
int main(int argc, char** argv) { ros::init(argc, argv, "dynamixel_mediator"); ros::NodeHandle nh; std::string user_to_track; std::string frame_id; PanController pan_controller(nh); ROS_INFO("Waiting for reference frame."); while(!ros::param::get("camera_frame_id", frame_id) && nh.ok()) { ros::Duration(1).sleep(); } ROS_INFO("Waiting for user identity."); while(!ros::param::get("user_to_track", user_to_track) && nh.ok()) { ros::Duration(1).sleep(); } user_to_track = user_to_track.substr(1, user_to_track.size()); std::string topic_to_subscribe("/kobra/tracker_cmd_vel"); nh.getParam("topic_to_subscribe", topic_to_subscribe); std::string topic_to_advertise("/kobra/cmd_vel"); nh.getParam("topic_to_advertise", topic_to_advertise); ros::Subscriber twistSubscriber = nh.subscribe(topic_to_subscribe, 1, twistCallback); pub = nh.advertise<geometry_msgs::Twist>(topic_to_advertise, 1); tf::TransformListener listener; tf::TransformBroadcaster broadcaster; std::deque<double> speed_to_rotate_left(MAX_MEAN, 0); std::deque<double> speed_to_rotate_right(MAX_MEAN, 0); ros::param::set("skeleton_to_track", skeleton_to_track); std::string direction = "still"; while(nh.ok()) { ROS_INFO("Looking for %s's face.", user_to_track.c_str()); std::string skeleton_to_track_frame; ros::Time reset = ros::Time::now(); while(nh.ok()) //Search continuously for a skeleton head very close to the designated user face. { std::vector<tf::StampedTransform> transforms; lookForEveryHeadTransform(listener, transforms, user_to_track); if(findClosestHeadToFace(transforms, skeleton_to_track_frame)) { skeleton_to_track = changeFrameAndReturnIndex(skeleton_to_track_frame); ros::param::set("skeleton_to_track", skeleton_to_track); break; } speed_to_rotate_left.pop_front(); speed_to_rotate_left.push_back(0); speed_to_rotate_right.pop_front(); speed_to_rotate_right.push_back(0); ratio = std::max(0.0, ratio - 0.005); ros::spinOnce(); if((ros::Time::now() - reset).sec >= 30 && !pan_controller.isHome()) { pan_controller.goHome(); } tf::Quaternion panOrientation; panOrientation.setRPY(0, 0, pan_controller.getRotation()); tf::Transform panTransform; panTransform.setOrigin(tf::Vector3(0, 0, 0.05)); panTransform.setRotation(panOrientation); broadcaster.sendTransform(tf::StampedTransform(panTransform, ros::Time::now(), "virgil_top_link", "pan_link")); ros::Rate(30).sleep(); } ROS_INFO("User %s and skeleton %s associated. Start tracking.", user_to_track.c_str(), skeleton_to_track_frame.c_str()); while(ros::param::get("skeleton_to_track", skeleton_to_track) && nh.ok()) { if(skeleton_to_track == 0) { ROS_INFO("User %s and skeleton %s association lost. Stop tracking.", user_to_track.c_str(), skeleton_to_track_frame.c_str()); pan_controller.standStill(); break; } tf::StampedTransform transform; std::string returnString = lookForSpecificBodyTransform(listener, frame_id, skeleton_to_track_frame, transform); double speed_to_add = newTwist.angular.z; if(returnString == "found") { //Ho la distanza, in base ad essa restituisco la percentuale di velocità del robot. double distance = std::sqrt(std::pow(transform.getOrigin().getX(), 2) + std::pow(transform.getOrigin().getY(), 2)); double alphaRAD = asin(transform.getOrigin().getY() / distance); double alphaDEG = alphaRAD / PI * 180; //Alpha = angolo espresso in gradi if(alphaDEG > 2) { speed_to_rotate_left.pop_front(); speed_to_rotate_left.push_back(((fabs(alphaDEG) - 2) / 180 * PI) * 3); speed_to_rotate_right = std::deque<double>(MAX_MEAN, 0); double speed = 0; for(int i = 0; i < MAX_MEAN; i++) { speed += speed_to_rotate_left[i] / MAX_MEAN; } if(speed_to_add > 0) { //Il robot sta girando a sinistra speed -= speed_to_add; } else { //Il robot sta girando a destra speed += speed_to_add; } if(speed < 0) { speed = -speed; pan_controller.turnRight(speed); direction = "right"; speed_to_rotate_right = speed_to_rotate_left; speed_to_rotate_left = std::deque<double>(MAX_MEAN, 0); } else { pan_controller.turnLeft(speed); direction = "left"; } } else if(alphaDEG < -2) { speed_to_rotate_right.pop_front(); speed_to_rotate_right.push_back(((fabs(alphaDEG) - 2) / 180 * PI) * 3); speed_to_rotate_left = std::deque<double>(MAX_MEAN, 0); double speed = 0; for(int i = 0; i < MAX_MEAN; i++) { speed += speed_to_rotate_right[i] / MAX_MEAN; } if(speed_to_add > 0) { //Il robot sta girando a sinistra speed += speed_to_add; } else { //Il robot sta girando a destra speed -= speed_to_add; } if(speed < 0) { speed = -speed; pan_controller.turnLeft(speed); direction = "left"; speed_to_rotate_left = speed_to_rotate_right; speed_to_rotate_right = std::deque<double>(MAX_MEAN, 0); } else { pan_controller.turnRight(speed); direction = "right"; } } else { speed_to_rotate_left = std::deque<double>(MAX_MEAN, 0); speed_to_rotate_right = std::deque<double>(MAX_MEAN, 0); pan_controller.standStill(), direction == "still"; } if(skeleton_to_track != -1) { if(distance >= 0 && distance <= 1.5) { ratio = 1; } else if(distance > 1.5) { ratio = std::max(1 - (distance - 1.5), 0.0); } if(ratio < 0) { ratio = 0; } else if(ratio > 1) { ratio = 1; } } else { ratio = std::max(0.0, ratio - 0.005); } } else if(returnString == "not found") { speed_to_rotate_left.pop_front(); speed_to_rotate_left.push_back(0.0); speed_to_rotate_right.pop_front(); speed_to_rotate_right.push_back(0.0); double speed_left = 0; double speed_right = 0; for(int i = 0; i < MAX_MEAN; i++) { speed_left += speed_to_rotate_left[i] / MAX_MEAN; speed_right += speed_to_rotate_right[i] / MAX_MEAN; } if(speed_left == 0 && speed_right == 0) { direction == "still"; pan_controller.standStill(); } if(direction == "left") { pan_controller.turnLeft(speed_left); } else if(direction == "right") { pan_controller.turnRight(speed_right); } ratio = std::max(0.0, ratio - 0.005); } else if(false && ((returnString == "skip" && skeleton_to_track == -1) || skeleton_to_track == -1)) { /* speed_to_rotate.pop_front(); speed_to_rotate.push_back(0.0); double speed = 0; for(int i = 0; i < MAX_MEAN; i++) { speed += speed_to_rotate[i] / MAX_MEAN; } if(speed == 0) { direction == "still"; pan_controller.standStill(); } if(direction == "left") { pan_controller.turnLeft(speed); } else if(direction == "right") { pan_controller.turnRight(speed); }*/ } ros::spinOnce(); tf::Quaternion panOrientation; panOrientation.setRPY(0, 0, pan_controller.getRotation()); tf::Transform panTransform; panTransform.setOrigin(tf::Vector3(0, 0, 0.05)); panTransform.setRotation(panOrientation); broadcaster.sendTransform(tf::StampedTransform(panTransform, ros::Time::now(), "virgil_top_link", "pan_link")); ros::Rate(30).sleep(); } } ros::shutdown(); exit(EXIT_SUCCESS); }
int main(int argc, char** argv) { ros::init(argc, argv, "dynamixel_mediator"); ros::NodeHandle nh; std::string user_to_track; //Stringa che conterrà l'ID dell'utente corretto, come da database, ricevuta al momento del login std::string frame_id; //Frame TF della camera ros::param::set("skeleton_to_track", skeleton_to_track); //Inizializzo a 0 l'ID dello scheletro da trackare ros::param::get("log_data", log_data); //Recupero la condizione di log dal file .launch PanController pan_controller(nh); //Istanza del controllore del pan-tilt ROS_INFO("Waiting for reference frame."); while(!ros::param::get("camera_frame_id", frame_id) && nh.ok()) //Leggo dai parametri ROS il frame della camera corretto { ros::Duration(1).sleep(); } std::string topic_to_subscribe("/kobra/tracker_cmd_vel"); //Topic da cui leggo i valori originali di velocità per il robot nh.getParam("topic_to_subscribe", topic_to_subscribe); //Aggiorno il topic nel caso sia diverso da quello scritto nella stringa std::string topic_to_advertise("/kobra/cmd_vel"); //Topic in cui andrò a spedire i valori mediati di velocità per il robot nh.getParam("topic_to_advertise", topic_to_advertise); //Aggiorno il topic nel caso sia diverso da quello scritto nella stringa ros::Subscriber twistSubscriber = nh.subscribe(topic_to_subscribe, 1, twistCallback); //Subscriber per ricevere i valori di velocità dal modulo di navigazione pub = nh.advertise<geometry_msgs::Twist>(topic_to_advertise, 1); //Publisher per spedire i valori di velocità mediati ros::Subscriber goalSubscriber = nh.subscribe("/move_base/status", 1, goalCallback); //Subscriber per leggere il goal corretto e il suo stato cancel = nh.advertise<actionlib_msgs::GoalID>("/move_base/cancel", 1); //Publisher per cancellare la action del move_base in caso di annullamento della sessione if(log_data) { logger = nh.advertise<std_msgs::String>("logger", 10); //Publisher per il logger, solo in caso di scelta di log positiva } tf::TransformListener listener; //Listener TF double ratio = 0; //Variabile che indica la percentuale con cui andrò a mediare la velocità del robot (numero reale da 0 a 1) std_msgs::String msg; //Messaggio utilizzato dal logger while(nh.ok()) //Ciclo padre, attende identità e goal e poi avvia l'accompagnamento { ROS_INFO("Waiting for user identity."); while(!ros::param::get("user_to_track", user_to_track) && nh.ok()) //Attendo fino a che non ricevo l'identità dell'utente da accompagnare { ros::Duration(1).sleep(); } user_to_track = user_to_track.substr(1, user_to_track.size()); //Depuro la stringa dall'underscore _ iniziale ROS_INFO("User identity received."); ROS_INFO("Waiting for goal."); while(current_goal_id == "") //Attendo l'arrivo del goal corretto { ros::spinOnce(); ros::Duration(1).sleep(); } ROS_INFO("Goal received."); if(log_data) //Effettuo il log di ID utente e goal { //Log identity, goal id and time msg.data = "start"; logger.publish(msg); msg.data = user_to_track; logger.publish(msg); msg.data = current_goal_id; logger.publish(msg); std::ostringstream sstream; sstream << goals_status[current_goal_id].first.sec << "." << goals_status[current_goal_id].first.nsec; msg.data = sstream.str(); logger.publish(msg); association_distances.clear(); } while(goals_status[current_goal_id].second != 3 && nh.ok()) //Ciclo dell'accompagnamento, che contiene le fasi di associazione, di tracking e riassociazione rapida { ROS_INFO("Looking for %s's face.", user_to_track.c_str()); std::string skeleton_to_track_frame; ros::Time reset = ros::Time::now(); //Timeout per il reset dell'orientamento della camera ros::Time abort = ros::Time::now(); //Timeout per annullare la sessione di accompagnamento bool restart = false; while(nh.ok()) //Ciclo che effettua l'associazione volto-scheletro { std::vector<tf::StampedTransform> transforms; //Vettore di TF che conterrà tutte le teste degli scheletri nel campo di vista lookForEveryHeadTransform(listener, transforms, user_to_track); //Cerco le teste degli scheletri if(findClosestHeadToFace(transforms, skeleton_to_track_frame)) //Controllo se ho una corrispondenza volto corretto-testa di uno scheletro { skeleton_to_track = changeFrameAndReturnIndex(skeleton_to_track_frame); //Recupero l'ID dello scheletro corretto sotto forma di intero (fino ad ora era su stringa) ros::param::set("skeleton_to_track", skeleton_to_track); //Comunico allo skeleton tracker l'ID dello scheletro di cui tenere traccia break; } ratio = std::max(0.0, ratio - 0.005); //Riduco gradualmente la percentuale di velocità da applicare alle ruote speed.pop_front(); //Elimino il campione più vecchio dalla media mobile speed.push_back(ratio); //Aggiungo il nuovo campione alla media mobile ros::spinOnce(); //Lascio agire le callback per impartire il moto al robot (in caso di moto residuo da una precedente associazione poi persa) if((ros::Time::now() - reset).sec >= CAMERA_RESET && !pan_controller.isHome()) //Tempo di reset dell'orientamento della camera scaduto { pan_controller.goHome(); //Mando la camera nella posizione centrale } if((ros::Time::now() - abort).sec >= SESSION_ABORT) //Non ho l'utente corretto da più di 2 minuti, annullo la sessione e mando il robot nella posizione iniziale. E' qui che posso lanciare eventuali allarmi o notificare al log l'accaduto. { ROS_INFO("Timout exceeded, restarting."); restart = true; break; } ros::Rate(30).sleep(); //Attendo che termini la finestra temporale del fotogramma } if(restart) //Effettuo il reset dell'algoritmo in caso di timeout { resetLoop(); break; } //A questo punto, se sono qui è perché ho associato un volto e uno scheletro di cui conosco l'ID, procedo nell'accompagnare l'utente ROS_INFO("User %s and skeleton %s associated. Start tracking.", user_to_track.c_str(), skeleton_to_track_frame.c_str()); while(ros::param::get("skeleton_to_track", skeleton_to_track) && goals_status[current_goal_id].second != 3 && nh.ok()) //Ciclo di accompagnamento { if(skeleton_to_track == 0) //Controllo subito un'eventuale perdita totale del tracking da parte dello skeleton tracker { ROS_INFO("User %s and skeleton %s association lost. Stop tracking.", user_to_track.c_str(), skeleton_to_track_frame.c_str()); pan_controller.standStill(); //Blocco la camera nella sua posizione attuale break; //Abbandono il ciclo di accompagnamento per ripartire dall'associazione volto-scheletro } tf::StampedTransform transform; std::string returnString = lookForSpecificBodyTransform(listener, frame_id, skeleton_to_track_frame, transform); //Cerco il torso dell'utente corretto o della stima del filtro di Kalman if(returnString == "found") //Ho trovato un nuovo set di coordinate del torso dell'utente { double distance = std::sqrt(std::pow(transform.getOrigin().getX(), 2) + std::pow(transform.getOrigin().getY(), 2)); //Calcolo la distanza tra la camera e l'utente corretto (planare e non spaziale) double alphaRAD = asin(transform.getOrigin().getY() / distance); //Angolo sotteso tra la direzione di vista della camera e la posizione dell'utente if(skeleton_to_track != -1) //Sto trackando l'utente e non la stima del filtro di kalman { if(distance >= 0 && distance <= 1.5) //Distanza dell'utente inferiore al metro e mezzo { ratio = 1; //Velocità massima } else if(distance > 1.5) //Distanza maggiore del metro e mezzo { ratio = std::max(1 - (distance - 1.5), 0.0); //Velocità mediata tra 1 e 0 (0 per distanze maggiori o uguali ai 2 metri e mezzo) } //Controllo per sicurezza il valore di percentuale velocità prima di muovere il robot if(ratio < 0) { ratio = 0; } else if(ratio > 1) { ratio = 1; } } else //Sto trackando la stima del filtro di kalman perché ho perso l'utente { ratio = std::max(0.0, ratio - 0.005); //Riduco gradualmente la velocità del robot } speed.pop_front(); //Aggiorno il filtro a media mobile speed.push_back(ratio); ros::spinOnce(); //Lascio agire le callback pan_controller.turn(alphaRAD, newTwist.angular.z); //Comunico al pan controller l'angolo tra il vettore di vista della camera e l'utente, unitamente alla rotazione effettuata dal robot per controcorreggerla } else if(returnString == "not found") //Non ho trovato le coordinate { ratio = std::max(0.0, ratio - 0.005); //Riduco gradualmente la velocità speed.pop_front(); //Aggiorno il filtro a media mobile speed.push_back(ratio); pan_controller.standStill(); //Fermo la camera ros::spinOnce(); //Lascio agire le callback } else if(returnString == "skip") //Ho trovato le coordinate ma sono le stesse trovate in precedenza, salto il calcolo { pan_controller.continueTurning(); //Proseguo con la precedente rotazione della camera ros::spinOnce(); //Lascio agire le callback } ros::Rate(30).sleep(); //Attendo fino al prossimo fotogramma } } if(goals_status[current_goal_id].second == 3) //Controllo se ho raggiunto il goal { ROS_INFO("Goal reached correctly, restarting."); if(log_data) //Mando al logger i dati raccolti { msg.data = "Association distances:"; logger.publish(msg); std::ostringstream sstream; for(int i = 0; i < association_distances.size(); i++) { sstream.clear(); sstream.str(std::string()); sstream << association_distances[i]; msg.data = sstream.str(); logger.publish(msg); } } ratio = 0; //Azzero la percentuale di velocità del robot resetLoop(); //Ri effettuo l'inizializzazione if(log_data) //Invio al logger la conferma per il salvataggio dei dati su file { msg.data = "succeeded"; logger.publish(msg); } //Dovrei mandare il robot alla posizione iniziale, ma non essendo definita ho saltato questa parte, basta una action a delle coordinate fisse } pan_controller.goHome(); //Resetto la posizione della camera } ROS_INFO("Shutting down."); ros::shutdown(); exit(EXIT_SUCCESS); }