diff --git a/cfg/IriStateMachineCompanion.cfg b/cfg/IriStateMachineCompanion.cfg
index 02bdaf66ff472f3e6a5a0d15798fc8382c9f4c6f..690959b0f4918fff361578150820bf810cf472b0 100755
--- a/cfg/IriStateMachineCompanion.cfg
+++ b/cfg/IriStateMachineCompanion.cfg
@@ -40,9 +40,14 @@ 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)", 3.5, 0.0, 30.0)
-gen.add("threshold_less_max_vel_robot_conf", double_t, 0, "less max vel of person because robot needs 0.2m/s more to react", 0.0, 0.0, 3.0)
+gen.add("threshold_max_dist_between_robot_and_person_conf", double_t, 0, "Maximum distance between robot and person to talk (meters)", 4.5, 0.0, 30.0)
+gen.add("threshold_less_max_vel_robot_conf", double_t, 0, "less max vel of person because robot needs 0.2m/s more to react", 0.2, 0.0, 3.0)
gen.add("threshold_minimun_distance_not_collide_conf", double_t, 0, "collision distance of the save_cmd_vel== 0.3", 0.8, 0.0, 3.0)
+gen.add("use_X_bool_conf" , bool_t, 0, "if true we use the X value of the final goal to return, if false we use the Y value.", True)
+gen.add("X_goal_to_return_conf", double_t, 0, " X of the goal in the environment to say return to the initial goal.", 45.0, 0.0, 300.0)
+gen.add("Y_goal_to_return_conf", double_t, 0, " Y of the goal in the environment to say return to the initial goal.", 20.5, 0.0, 300.0)
+gen.add("threshold_to_return_conf", double_t, 0, " threshold distance to say the return back, from the return goal.", 3.0, 0.0, 300.0)
+
exit(gen.generate(PACKAGE, "IriStateMachineCompanionAlgorithm", "IriStateMachineCompanion"))
diff --git a/include/iri_state_machine_companion_alg_node.h b/include/iri_state_machine_companion_alg_node.h
index 7f6eae162c4f27d282e2bbe73c66e2c1f158d065..cb92e969f129a252a2795f4f9cc3af59a464251f 100644
--- a/include/iri_state_machine_companion_alg_node.h
+++ b/include/iri_state_machine_companion_alg_node.h
@@ -96,6 +96,12 @@ class IriStateMachineCompanionAlgNode : public algorithm_base::IriBaseAlgorithm<
unsigned int id_person_companion_, id_second_person_companion_;
double threshold_max_dist_between_robot_and_person_,threshold_less_max_vel_robot_,threshold_minimun_distance_not_collide_;
+
+ Spoint final_goal_to_return_;
+ bool use_x_;
+ double threshold_to_return_;
+
+ bool out_std_mesages_;
// [service attributes]
// [client attributes]
diff --git a/src/iri_state_machine_companion_alg_node.cpp b/src/iri_state_machine_companion_alg_node.cpp
index f1499b361a2751b1446bc40d1bf54ee0f37dac64..57909853cea7bb69ae6f600f4dc785c63b7844db 100644
--- a/src/iri_state_machine_companion_alg_node.cpp
+++ b/src/iri_state_machine_companion_alg_node.cpp
@@ -2,7 +2,8 @@
IriStateMachineCompanionAlgNode::IriStateMachineCompanionAlgNode(void) :
algorithm_base::IriBaseAlgorithm<IriStateMachineCompanionAlgorithm>(),
- tf_(ros::Duration(10.f))
+ tf_(ros::Duration(10.f)),
+ out_std_mesages_(false)
//this->fixed_frame("/map"),
//this->robot_frame("/tibi/base_link")
{
@@ -95,9 +96,62 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void)
//ROS_INFO("save_companionStatus_mesage_global_.numCompanionPeople=%d",save_companionStatus_mesage_global_.numCompanionPeople);
if(actual_numCompanionPeople==0){
this->tibi_akp_status_UInt64_msg_.data =1; // status no people.
+ if(out_std_mesages_){
+ ROS_INFO(" IN status no people (1) ");
+ }
}
// TODO status fake person ==2 and status return other goal ==3
+ // INI: implementar status return other goal ==3
+
+ use_x_=config_.use_X_bool_conf;
+ threshold_to_return_=config_.threshold_to_return_conf;
+ final_goal_to_return_=Spoint(config_.X_goal_to_return_conf,config_.Y_goal_to_return_conf);
+
+ Spoint position_p1,position_p2;
+ for(unsigned int u=0;u<this->obs.size();u++){
+ if(this->obs[u].id==id_person_companion_){
+ position_p1=Spoint(this->obs[u].x,this->obs[u].y,this->obs[u].time_stamp);
+ }
+ if(this->obs[u].id==id_second_person_companion_){
+ position_p2=Spoint(this->obs[u].x,this->obs[u].y,this->obs[u].time_stamp);
+ }
+ }
+
+ if(out_std_mesages_){
+ std::cout<< "(status return_vuelta) id_person_companion_=" <<id_person_companion_<<"; position_p1.x="<<position_p1.x<<"; position_p1.y="<<position_p1.y<< std::endl;
+ std::cout<< "(status return_vuelta) id_second_person_companion_=" <<id_second_person_companion_<<"; position_p1.x="<<position_p1.x<<"; position_p1.y="<<position_p1.y<< std::endl;
+ std::cout<< "(status return_vuelta) robot.x=" <<this->robot_pose_.x<<"; this->robot_pose_.y="<<this->robot_pose_.y<< std::endl;
+ }
+
+ // double actual_per_rob_dist=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));
+ if(use_x_){
+ double dist_goal_robot=sqrt((this->robot_pose_.x-final_goal_to_return_.x)*(this->robot_pose_.x-final_goal_to_return_.x));
+ double dist_goal_p1=sqrt((position_p1.x-final_goal_to_return_.x)*(position_p1.x-final_goal_to_return_.x));
+ double dist_goal_p2=sqrt((position_p2.x-final_goal_to_return_.x)*(position_p2.x-final_goal_to_return_.x));
+ if((dist_goal_robot<threshold_to_return_)){
+ this->tibi_akp_status_UInt64_msg_.data =3;
+
+ if(out_std_mesages_){
+ ROS_INFO(" IN status goal return (3-x) ");
+ }
+ }
+
+ }else{
+ double dist_goal_robot=sqrt((this->robot_pose_.y-final_goal_to_return_.y)*(this->robot_pose_.y-final_goal_to_return_.y));
+ double dist_goal_p1=sqrt((position_p1.y-final_goal_to_return_.y)*(position_p1.y-final_goal_to_return_.y));
+ double dist_goal_p2=sqrt((position_p2.y-final_goal_to_return_.y)*(position_p2.y-final_goal_to_return_.y));
+ if((dist_goal_robot<threshold_to_return_)){
+ this->tibi_akp_status_UInt64_msg_.data =3;
+
+ if(out_std_mesages_){
+ ROS_INFO(" IN status goal return (3-y) ");
+ }
+ }
+ }
+
+ // FIN: implementar status return other goal ==3
+
bool aboiding_stat_obst = save_companionStatus_mesage_global_.estoyEvitandoObstaculosEstaticos;
//ROS_INFO("save_companionStatus_mesage_global_.estoyEvitandoObstaculosEstaticos=%d",save_companionStatus_mesage_global_.estoyEvitandoObstaculosEstaticos);
if(aboiding_stat_obst==true){
@@ -131,6 +185,11 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void)
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));
+
+
+ if(out_std_mesages_){
+ std::cout<< "(person_companion_velocity) 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;
+ }
}
//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;
@@ -144,8 +203,11 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void)
threshold_less_max_vel_robot_=config_.threshold_less_max_vel_robot_conf;
double max_robot_velocity=save_companionStatus_mesage_global_.maximumRobotVelocity;
- if((max_robot_velocity - threshold_less_max_vel_robot_) < person_companion_velocity){
+ if(((max_robot_velocity - threshold_less_max_vel_robot_) < person_companion_velocity)&&(person_companion_velocity>0.0)){
this->tibi_akp_status_UInt64_msg_.data =7; // status walk slowly, please. I can not follow you.
+ if(out_std_mesages_){
+ ROS_INFO(" IN status walk slowly (7); max_robot_velocity=%f ; threshold_less_max_vel_robot_=%f ; person_companion_velocity=%f",max_robot_velocity,threshold_less_max_vel_robot_,person_companion_velocity);
+ }
}
threshold_max_dist_between_robot_and_person_=config_.threshold_max_dist_between_robot_and_person_conf;
@@ -154,6 +216,11 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void)
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.
+
+ if(out_std_mesages_){
+ ROS_INFO(" IN status wait for me (8) ");
+ }
+
}
threshold_minimun_distance_not_collide_=config_.threshold_minimun_distance_not_collide_conf;
@@ -163,13 +230,21 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void)
}*/
if(save_companionStatus_mesage_global_.estateNoPathObstacleClose){
+ if(out_std_mesages_){
ROS_INFO("NO PATH STATUS save_companionStatus_mesage_global_.estateNoPathObstacleClose=%d",save_companionStatus_mesage_global_.estateNoPathObstacleClose);
+ }
this->tibi_akp_status_UInt64_msg_.data =9; //obstacle very close.
+
+ if(out_std_mesages_){
+ ROS_INFO(" IN obstacle very close (9) ");
+ }
}
//this->tibi_akp_status_UInt64_msg_.data = my_var;
-
+ if(out_std_mesages_){
+ ROS_INFO("STATUS state machine!!! tibi_akp_status_=%d",this->tibi_akp_status_UInt64_msg_.data );
+ }
// [fill srv structure and make request to the server]
// [fill action structure and make request to the action server]