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