quitando algunps scout para test en IVO-real

cfg/IriStateMachineCompanion.cfg

@@ -46,6 +46,8 @@ gen.add("frame_robot_footprint",    str_t,     0, "frame to transform poses to",
 gen.add("robot",    str_t,     0, "frame to transform poses to", "tiago") 
 gen.add("conf_bool_test_case_head1"       ,   bool_t,  0, "test head left or right.", True)
 gen.add("out_std_mesages_conf"       ,   bool_t,  0, "if true we se the internal std_out mesages.", False)
+gen.add("out_state_machine_status_mesage_conf"       ,   bool_t,  0, "if true we se the final state of this state machine, the state that the message outputs from this node.", False)
+
 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)

include/iri_state_machine_companion_alg_node.h

@@ -103,6 +103,7 @@ class IriStateMachineCompanionAlgNode : public algorithm_base::IriBaseAlgorithm<
     double threshold_to_return_;
 
     bool out_std_mesages_;
+    bool out_state_machine_status_mesage_;
     bool bool_test_case_head1_;
 
     //differenciate states of group accompaniment and terrined project of 1 person accompaniment

src/iri_state_machine_companion_alg_node.cpp

@@ -94,6 +94,7 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void)
 	this->tibi_akp_status_UInt64_msg_.data =0; // case no status.
 
 	out_std_mesages_=config_.out_std_mesages_conf;
+	out_state_machine_status_mesage_=config_.out_state_machine_status_mesage_conf;
 	    
     	//terrinet_project_=config_.terrinet_project_conf; //differenciate states of group accompaniment and terrined project of 1 person accompaniment
 
@@ -137,18 +138,23 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void)
 	  			if( diffangle(theta, angle) < 0 )
 	  			{
 	    			  this->tibi_akp_status_UInt64_msg_.data =11; // por ejemplo right=11.
-					std::cout<< " iri_state_machine; diff<0; status=11" << std::endl;
+					if(out_std_mesages_){
+					std::cout<< " iri_state_machine; diff<0; status=11" << std::endl;}
 	  			}else{
-					std::cout<< " iri_state_machine; diff>0; status=12" << std::endl;
+					if(out_std_mesages_){
+					std::cout<< " iri_state_machine; diff>0; status=12" << std::endl;}
 	    			  this->tibi_akp_status_UInt64_msg_.data =12; // por ejemplo left =12.
 	  			}
 			}else{
 	  			if( diffangle(theta, angle) < 0 )
 	  			{
-				std::cout<< " iri_state_machine; diff<0; status=12" << std::endl;
-	    			  this->tibi_akp_status_UInt64_msg_.data =12;
+					if(out_std_mesages_){
+					std::cout<< " iri_state_machine; diff<0; status=12" << std::endl;}
+	    			  
+				this->tibi_akp_status_UInt64_msg_.data =12;
 	  			}else{
-				std::cout<< " iri_state_machine; diff>0; status=11" << std::endl;
+					if(out_std_mesages_){
+					std::cout<< " iri_state_machine; diff>0; status=11" << std::endl;}
 	    			  this->tibi_akp_status_UInt64_msg_.data =11;
 	  			}
 			}
@@ -303,8 +309,9 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void)
 
   //this->tibi_akp_status_UInt64_msg_.data = my_var;
 
-	if(out_std_mesages_){
+	if(out_state_machine_status_mesage_){
   		//ROS_INFO("STATUS state machine!!! tibi_akp_status_=%d",this->tibi_akp_status_UInt64_msg_.data );
+	
 	std::cout<< "STATUS state machine!!! tibi_akp_status_=" << this->tibi_akp_status_UInt64_msg_.data << std::endl;
 	}
   // [fill srv structure and make request to the server]