set up last exp tibi, 2019 octubre

cfg/IriStateMachineCompanion.cfg

@@ -40,13 +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("conf_bool_test_case_head1"       ,   bool_t,  0, "test head left or right.", False)
 gen.add("out_std_mesages_conf"       ,   bool_t,  0, "if true we se the internal std_out mesages.", 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)
 
 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("X_goal_to_return_conf",  double_t,  0,    " X of the goal in the environment to say return to the initial goal.",  42.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)
 

include/iri_state_machine_companion_alg_node.h

@@ -102,6 +102,7 @@ class IriStateMachineCompanionAlgNode : public algorithm_base::IriBaseAlgorithm<
     double threshold_to_return_;
 
     bool out_std_mesages_;
+    bool bool_test_case_head1_;
     // [service attributes]
 
     // [client attributes]

src/iri_state_machine_companion_alg_node.cpp

@@ -125,6 +125,36 @@ void IriStateMachineCompanionAlgNode::mainNodeThread(void)
 		std::cout<< "(status return_vuelta) robot.x=" <<this->robot_pose_.x<<"; this->robot_pose_.y="<<this->robot_pose_.y<< std::endl;
 	}
 
+
+	// INI calculate side where robot need to look
+		if(save_companionStatus_mesage_global_.robotLateral){ //robot lateral
+			 
+  			double angle=atan2(this->robot_pose_.y-position_p1.y , this->robot_pose_.x-position_p1.x);
+  			double theta =atan2(final_goal_to_return_.y-position_p1.y , final_goal_to_return_.x-position_p1.x);
+			bool_test_case_head1_=config_.conf_bool_test_case_head1;
+			if(bool_test_case_head1_){
+	  			if( diffangle(theta, angle) < 0 )
+	  			{
+	    			  this->tibi_akp_status_UInt64_msg_.data =11; // por ejemplo right=11.
+	  			}else{
+	    			  this->tibi_akp_status_UInt64_msg_.data =12; // por ejemplo left =12.
+	  			}
+			}else{
+	  			if( diffangle(theta, angle) < 0 )
+	  			{
+	    			  this->tibi_akp_status_UInt64_msg_.data =12;
+	  			}else{
+	    			  this->tibi_akp_status_UInt64_msg_.data =11;
+	  			}
+			}
+
+		}else{ //robot central
+			this->tibi_akp_status_UInt64_msg_.data =13;
+		}
+
+	// FIN calculate side where robot need to look
+
+
 	// 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));