diff --git a/cfg/IriStateMachineCompanion.cfg b/cfg/IriStateMachineCompanion.cfg
index 07e40d3752d72edb8d9dd4460e2a146d4d8d0f55..9d1eedbaf9b2c1c73721526141f0ac5ca6a5182e 100755
--- a/cfg/IriStateMachineCompanion.cfg
+++ b/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)
 
diff --git a/include/iri_state_machine_companion_alg_node.h b/include/iri_state_machine_companion_alg_node.h
index cb92e969f129a252a2795f4f9cc3af59a464251f..5babbbe1d4b6f82f579995b727ddf9754b37d759 100644
--- a/include/iri_state_machine_companion_alg_node.h
+++ b/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]
diff --git a/src/iri_state_machine_companion_alg_node.cpp b/src/iri_state_machine_companion_alg_node.cpp
index 0e0fd4a38e1fca71badcc893d8bf19fd5458212f..3dd27c8759e61548b6b6a58daa5ea33769400b8e 100644
--- a/src/iri_state_machine_companion_alg_node.cpp
+++ b/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));