Skip to content
Snippets Groups Projects
Commit 3c4010f3 authored by Ely Repiso Polo's avatar Ely Repiso Polo
Browse files

arreglando state machine companion talk status

parent 11f92399
No related branches found
No related tags found
No related merge requests found
......@@ -39,5 +39,8 @@ gen = ParameterGenerator()
# Name Type Reconfiguration level Description Default Min Max
#gen.add("velocity_scale_factor", double_t, 0, "Maximum velocity scale factor", 0.5, 0.0, 1.0)
# threshold_max_dist_between_robot_and_person_
gen.add("threshold_max_dist_between_robot_and_person_conf", double_t, 0, "Maximum distance between robot and person to talk (meters)", 5.0, 0.0, 30.0)
exit(gen.generate(PACKAGE, "IriStateMachineCompanionAlgorithm", "IriStateMachineCompanion"))
......@@ -57,6 +57,9 @@
*/
class IriStateMachineCompanionAlgNode : public algorithm_base::IriBaseAlgorithm<IriStateMachineCompanionAlgorithm>
{
iri_nav_msgs::companionStateResults save_companionStatus_mesage_global_;
private:
// normal variables:
......@@ -77,6 +80,7 @@ class IriStateMachineCompanionAlgNode : public algorithm_base::IriBaseAlgorithm<
pthread_mutex_t companionState_mutex_;
void companionState_mutex_enter(void);
void companionState_mutex_exit(void);
ros::Subscriber odom_subscriber_;
void odom_callback(const nav_msgs::Odometry::ConstPtr& msg);
......@@ -90,6 +94,8 @@ class IriStateMachineCompanionAlgNode : public algorithm_base::IriBaseAlgorithm<
void people_tracks_mutex_enter(void);
void people_tracks_mutex_exit(void);
unsigned int id_person_companion_, id_second_person_companion_;
double threshold_max_dist_between_robot_and_person_;
// [service attributes]
......
......@@ -22,7 +22,7 @@ IriStateMachineCompanionAlgNode::IriStateMachineCompanionAlgNode(void) :
this->people_tracks_subscriber_ = this->public_node_handle_.subscribe("people_tracks", 100, &IriStateMachineCompanionAlgNode::people_tracks_callback, this);
pthread_mutex_init(&this->people_tracks_mutex_,NULL);
threshold_max_dist_between_robot_and_person_=5.0;
// [init services]
// [init clients]
......@@ -44,6 +44,92 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void)
{
// [fill msg structures]
// Initialize the topic message structure
/* //if no_people poner .numCompanionPeople=0; // si no encuentro los ids de las person companion
if((!to_stop_we_have_p1_)&&(!to_stop_we_have_p2_)){
ROS_ERROR(" NO ID's!!! NOOOO PEOPLE!!! ");
// estado. No os veo. Volver a colocaros en la paosición para continuar.
// ESTADO planner.
if(!stop_robot_manually_){
companionState_companionStateResults_msg_.numCompanionPeople=0; // if no_people poner .numCompanionPeople=0; // si no encuentro los ids de las person companion
}
}else{
companionState_companionStateResults_msg_.numCompanionPeople=number_of_people_in_group_;
}
this->companionState_companionStateResults_msg_.idsCompanionPeople.resize(0);
if(number_of_people_in_group_==1){
this->companionState_companionStateResults_msg_.idsCompanionPeople.resize(1);
companionState_companionStateResults_msg_.idsCompanionPeople[0]=id_person_companion_;
}else if(number_of_people_in_group_>1){
this->companionState_companionStateResults_msg_.idsCompanionPeople.resize(2);
companionState_companionStateResults_msg_.idsCompanionPeople[0]=id_person_companion_;
companionState_companionStateResults_msg_.idsCompanionPeople[1]=id_SECOND_person_companion_;
}
companionState_companionStateResults_msg_.maximumRobotVelocity=this->planner_.get_max_v_by_system();
Sdestination robot_actual_dest=this->planner_.get_robot_destination();
companionState_companionStateResults_msg_.destinations.resize(1);
iri_nav_msgs::destination actual_dest_pers_robot;
actual_dest_pers_robot.personid=id_person_companion_;
actual_dest_pers_robot.position.x=robot_actual_dest.x;
actual_dest_pers_robot.position.y=robot_actual_dest.y;
actual_dest_pers_robot.position.z=0.0;
companionState_companionStateResults_msg_.destinations[0]=actual_dest_pers_robot;
companionState_companionStateResults_msg_.estoyEvitandoObstaculosEstaticos=this->planner_.get_bool_evitando_obst_estaticos();
companionState_companionStateResults_msg_.estoyEvitandoObstaculosDynamicos=this->planner_.get_bool_evitando_obst_dynamicos();
this->companionState_publisher_.publish(this->companionState_companionStateResults_msg_); */
this->tibi_akp_status_UInt64_msg_.data =0; // case no status.
unsigned int actual_numCompanionPeople=save_companionStatus_mesage_global_.numCompanionPeople;
if(actual_numCompanionPeople==0){
this->tibi_akp_status_UInt64_msg_.data =1; // status no people.
}
// TODO status fake person ==2 and status return other goal ==3
bool aboiding_stat_obst = save_companionStatus_mesage_global_.estoyEvitandoObstaculosEstaticos;
if(aboiding_stat_obst==true){
this->tibi_akp_status_UInt64_msg_.data =4; // aboiding static obstacles
}
bool aboiding_dyn_obst = save_companionStatus_mesage_global_.estoyEvitandoObstaculosDynamicos;
if(aboiding_dyn_obst==true){
this->tibi_akp_status_UInt64_msg_.data =5; // aboiding dynamic obstacles
}
if(!save_companionStatus_mesage_global_.idsCompanionPeople.empty()){
id_person_companion_=save_companionStatus_mesage_global_.idsCompanionPeople[0];
if(save_companionStatus_mesage_global_.idsCompanionPeople.size()>1){
id_second_person_companion_=save_companionStatus_mesage_global_.idsCompanionPeople[1];
}
}
// TODO: status side-by-side. ==6
double person_companion_velocity=0.0;
double distance_robot_person_comp;
for(unsigned int u=0;u<this->obs.size();u++){
if(this->obs[u].id==id_person_companion_){
person_companion_velocity=sqrt(this->obs[u].vx*this->obs[u].vx+this->obs[u].vy*this->obs[u].vy);
distance_robot_person_comp=sqrt((this->obs[u].x - this->robot_pose_.x)*(this->obs[u].x -this->robot_pose_.x)+(this->obs[u].y - this->robot_pose_.y)*(this->obs[u].y -this->robot_pose_.y));
}
//std::cout<< "this->obs; id=" <<this->obs[u].id<<"; x="<<this->obs[u].x<<"; y="<<this->obs[u].y<<"; vx="<<this->obs[u].vx<<"; vy="<<this->obs[u].vy<< std::endl;
}
double max_robot_velocity=save_companionStatus_mesage_global_.maximumRobotVelocity;
if(max_robot_velocity < person_companion_velocity){
this->tibi_akp_status_UInt64_msg_.data =7; // status walk slowly, please. I can not follow you.
}
threshold_max_dist_between_robot_and_person_=config_.threshold_max_dist_between_robot_and_person_conf;
if(distance_robot_person_comp > threshold_max_dist_between_robot_and_person_){
this->tibi_akp_status_UInt64_msg_.data =8; //status wait for me, please.
}
//this->tibi_akp_status_UInt64_msg_.data = my_var;
......@@ -66,6 +152,9 @@ void IriStateMachineCompanionAlgNode::companionState_callback(const iri_nav_msgs
//this->alg_.lock();
//this->companionState_mutex_enter();
save_companionStatus_mesage_global_=*msg;
//std::cout << msg->data << std::endl;
//unlock previously blocked shared variables
//this->alg_.unlock();
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment