test en tibi side-by-side 2pers

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"))
 

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]

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]