mejorado evitar obstaculos estaticos y dinamicos + add un nuevo estado en el...

mejorado evitar obstaculos estaticos y dinamicos + add un nuevo estado en el state machine + que diga iterativamente que no tiene gente. Ese ultimo a adaptar el rato que tarda en volver a decirlo 

cfg/IriStateMachineCompanion.cfg

@@ -41,6 +41,8 @@ gen = ParameterGenerator()
 #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.2,      0.0,  3.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_minimun_distance_not_collide_conf",  double_t,  0,    "collision distance of the save_cmd_vel== 0.3",  0.8,      0.0,  3.0)
+
 exit(gen.generate(PACKAGE, "IriStateMachineCompanionAlgorithm", "IriStateMachineCompanion"))
 

include/iri_state_machine_companion_alg_node.h

@@ -95,8 +95,7 @@ class IriStateMachineCompanionAlgNode : public algorithm_base::IriBaseAlgorithm<
     void people_tracks_mutex_exit(void);
 
     unsigned int id_person_companion_, id_second_person_companion_;
-    double threshold_max_dist_between_robot_and_person_;
-    double threshold_less_max_vel_robot_;
+    double threshold_max_dist_between_robot_and_person_,threshold_less_max_vel_robot_,threshold_minimun_distance_not_collide_;
     // [service attributes]
 
     // [client attributes]

src/iri_state_machine_companion_alg_node.cpp

@@ -122,12 +122,22 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void)
 	// TODO: status side-by-side.  ==6
 	double person_companion_velocity=0.0;
 	double distance_robot_person_comp;
+	
+	double minimum_distance=10;
 	for(unsigned int u=0;u<this->obs.size();u++){
+		if((u==0)&&(this->obs[u].id!=id_person_companion_)){
+			minimum_distance=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(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 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((actual_per_rob_dist<minimum_distance)&&(this->obs[u].id!=id_person_companion_)){ // la person companion no cuenta. Me faltará caso obst staticos...
+			minimum_distance=actual_per_rob_dist;
+		}
  	}
 
 	//ROS_INFO("save_companionStatus_mesage_global_.maximumRobotVelocity=%f; person_companion_velocity=%f",save_companionStatus_mesage_global_.maximumRobotVelocity,person_companion_velocity);
@@ -145,6 +155,17 @@ 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.
 	}
+
+	threshold_minimun_distance_not_collide_=config_.threshold_minimun_distance_not_collide_conf;	
+	// new status:  I can not move!, Sorry.
+	/*if((minimum_distance < threshold_minimun_distance_not_collide_)&&(this->robot_pose_.v<0.2)){
+		this->tibi_akp_status_UInt64_msg_.data =9; //obstacle very close.
+	}*/
+
+	if(save_companionStatus_mesage_global_.estateNoPathObstacleClose){
+		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.
+	}
 	
   //this->tibi_akp_status_UInt64_msg_.data = my_var;