cambiados limites hablar cuando se aleja persona, que estaban muy altos

cfg/IriStateMachineCompanion.cfg

@@ -40,7 +40,7 @@ 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)
-
+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.2,      0.0,  3.0)
 exit(gen.generate(PACKAGE, "IriStateMachineCompanionAlgorithm", "IriStateMachineCompanion"))
 

include/iri_state_machine_companion_alg_node.h

@@ -96,7 +96,7 @@ class IriStateMachineCompanionAlgNode : public algorithm_base::IriBaseAlgorithm<
 
     unsigned int id_person_companion_, id_second_person_companion_;
     double threshold_max_dist_between_robot_and_person_;
-
+    double threshold_less_max_vel_robot_;
     // [service attributes]
 
     // [client attributes]

src/iri_state_machine_companion_alg_node.cpp

@@ -8,7 +8,7 @@ IriStateMachineCompanionAlgNode::IriStateMachineCompanionAlgNode(void) :
 {
   //init class attributes if necessary
   this->loop_rate_ = 2;//in [Hz] TODO: ver a cuantos ercios ir.
-ROS_INFO("ini node");
+//ROS_INFO("ini node");
   this->fixed_frame = "/map";
   this->robot_frame  = "/tibi/base_link";
 
@@ -31,7 +31,7 @@ ROS_INFO("ini node");
   // [init clients]
   
   // [init action servers]
-ROS_INFO("node created");
+//ROS_INFO("node created");
   // [init action clients]
 }
 
@@ -92,30 +92,30 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void)
 
 	
 	unsigned int actual_numCompanionPeople=save_companionStatus_mesage_global_.numCompanionPeople;
-	ROS_INFO("save_companionStatus_mesage_global_.numCompanionPeople=%d",save_companionStatus_mesage_global_.numCompanionPeople);
+	//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.
 	}
 	
 	// TODO status fake person ==2 and 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);
+	//ROS_INFO("save_companionStatus_mesage_global_.estoyEvitandoObstaculosEstaticos=%d",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;
-	ROS_INFO("save_companionStatus_mesage_global_.estoyEvitandoObstaculosDynamicos=%d",save_companionStatus_mesage_global_.estoyEvitandoObstaculosDynamicos);
+	//ROS_INFO("save_companionStatus_mesage_global_.estoyEvitandoObstaculosDynamicos=%d",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];
-		ROS_INFO("id_person_companion_=%d",id_person_companion_);
+		//ROS_INFO("id_person_companion_=%d",id_person_companion_);
 		if(save_companionStatus_mesage_global_.idsCompanionPeople.size()>1){
 			id_second_person_companion_=save_companionStatus_mesage_global_.idsCompanionPeople[1];
-			ROS_INFO("id_second_person_companion_=%d",id_second_person_companion_);
+			//ROS_INFO("id_second_person_companion_=%d",id_second_person_companion_);
 		}
 	}
 
@@ -130,14 +130,17 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void)
 		//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;
  	}
 
-	ROS_INFO("save_companionStatus_mesage_global_.maximumRobotVelocity=%f; person_companion_velocity=%f",save_companionStatus_mesage_global_.maximumRobotVelocity,person_companion_velocity);
+	//ROS_INFO("save_companionStatus_mesage_global_.maximumRobotVelocity=%f; person_companion_velocity=%f",save_companionStatus_mesage_global_.maximumRobotVelocity,person_companion_velocity);
+	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 < person_companion_velocity){
+	if((max_robot_velocity - threshold_less_max_vel_robot_) < 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;
-	ROS_INFO("threshold_max_dist_between_robot_and_person_=%f; distance_robot_person_comp=%f",threshold_max_dist_between_robot_and_person_,distance_robot_person_comp);
+	
+	//ROS_INFO("threshold_max_dist_between_robot_and_person_=%f; distance_robot_person_comp=%f",threshold_max_dist_between_robot_and_person_,distance_robot_person_comp);
 	
 	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.
@@ -159,7 +162,7 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void)
 /*  [subscriber callbacks] */
 void IriStateMachineCompanionAlgNode::companionState_callback(const iri_nav_msgs::companionStateResults::ConstPtr& msg)
 {
-  ROS_INFO("IriStateMachineCompanionAlgNode::companionState_callback: New Message Received");
+  //ROS_INFO("IriStateMachineCompanionAlgNode::companionState_callback: New Message Received");
 
   //use appropiate mutex to shared variables if necessary
   //this->alg_.lock();
@@ -186,7 +189,7 @@ void IriStateMachineCompanionAlgNode::companionState_mutex_exit(void)
 
 void IriStateMachineCompanionAlgNode::odom_callback(const nav_msgs::Odometry::ConstPtr& msg)
 {
-  ROS_INFO("IriStateMachineCompanionAlgNode::odom_callback: New Message Received");
+  //ROS_INFO("IriStateMachineCompanionAlgNode::odom_callback: New Message Received");
 
   //use appropiate mutex to shared variables if necessary
   //this->alg_.lock();
@@ -276,7 +279,7 @@ void IriStateMachineCompanionAlgNode::odom_mutex_exit(void)
 
 void IriStateMachineCompanionAlgNode::people_tracks_callback(const iri_perception_msgs::detectionArray::ConstPtr& msg)
 {
-  ROS_INFO("IriStateMachineCompanionAlgNode::people_tracks_callback: New Message Received");
+  //ROS_INFO("IriStateMachineCompanionAlgNode::people_tracks_callback: New Message Received");
 
   //use appropiate mutex to shared variables if necessary
   //this->alg_.lock();