diff --git a/humanoid_modules/include/humanoid_modules/tracking_module.h b/humanoid_modules/include/humanoid_modules/tracking_module.h
index bbb02baaa0d14c871152ecb5ecbfe1dbacb8f606..a66088e40da76b43122f00b04a49591ddb54a1cd 100644
--- a/humanoid_modules/include/humanoid_modules/tracking_module.h
+++ b/humanoid_modules/include/humanoid_modules/tracking_module.h
@@ -41,8 +41,8 @@
typedef enum {TRACK_MODULE_IDLE,
TRACK_MODULE_TRACK,
- TRACK_MODULE_APPROACH,
TRACK_MODULE_WAIT,
+ ORIENTATE,
TRACK_MODULE_END} tracking_module_states_t;
typedef enum {TRACK_MODULE_SUCCESS,
@@ -108,6 +108,7 @@ class CTrackingModule : public CModule<tracking_module::TrackingModuleConfig>
TPID z_distance;
TPID x_distance;
TPID orientation;
+ double yaw_target;
TPID orientation_x;
ros::Time start_time;
ros::Time new_time;
diff --git a/humanoid_modules/src/tracking_module.cpp b/humanoid_modules/src/tracking_module.cpp
index d39428c403d8d719994c38f441a92b87600ce687..6999037756e6250e25f826407a0c8fcfec40907d 100644
--- a/humanoid_modules/src/tracking_module.cpp
+++ b/humanoid_modules/src/tracking_module.cpp
@@ -80,7 +80,7 @@ void CTrackingModule::state_machine(void)
case TRACK_MODULE_IDLE: ROS_INFO("CTrackingModule: state IDLE");
if(this->new_track)
{
- this->state=TRACK_MODULE_TRACK;
+ this->state=TRACK_MODULE_WAIT;
this->status=TRACK_MODULE_RUNNING;
this->tracking=true;
head_tracking_module.start_tracking(current_pan_angle, current_tilt_angle);
@@ -91,6 +91,13 @@ void CTrackingModule::state_machine(void)
this->state=TRACK_MODULE_IDLE;
break;
+
+ case TRACK_MODULE_WAIT: ROS_INFO("CTrackingModule: state WAIT");
+ if(qr_detected)
+ this->state=TRACK_MODULE_TRACK;
+ else
+ this->state=TRACK_MODULE_WAIT;
+ break;
/*
case TRACK_MODULE_WAIT: ROS_INFO("CTrackingModule: state WAIT");
if(head_tracking_module.is_finished())
@@ -107,9 +114,9 @@ void CTrackingModule::state_machine(void)
break;
//Comprobar si se ha perdido QR
- case TRACK_MODULE_TRACK: ROS_INFO("CTrackingModule: state TRACK");
+ case TRACK_MODULE_WAIT: ROS_INFO("CTrackingModule: state WAIT");
if(qr_detected)
- this->state=TRACK_MODULE_APPROACH;
+ this->state=TRACK_MODULE_TRACK;
else
{
if(this->watchdog_qr_lost.is_active())
@@ -275,7 +282,7 @@ void CTrackingModule::state_machine(void)
walk_module.set_steps_size(z_distance.pid_out,x_distance.pid_out,orientation.pid_out);
break;
*/
-
+ /*
case TRACK_MODULE_TRACK: ROS_INFO("CTrackingModule: state TRACK");
// Update pid
@@ -300,7 +307,7 @@ void CTrackingModule::state_machine(void)
pid_yaw(orientation); //mover yaw dependiendo de distancia x
// x_distance.pid_out=0.0;
}
- if(orientation.pid_out==0.0 && z_distance.pid_out==0.0/*&& x_distance.pid_out==0.0*/)
+ if(orientation.pid_out==0.0 && z_distance.pid_out==0.0*/ /*&& x_distance.pid_out==0.0*/ /* )
{
ROS_INFO("CTrackingModule: Stop aproaching");
walk_module.stop();
@@ -342,10 +349,97 @@ void CTrackingModule::state_machine(void)
this->status=TRACK_MODULE_LOST;
}
+ break; */
+
+ case TRACK_MODULE_TRACK: ROS_INFO("CTrackingModule: state TRACK");
+
+ // Update pid
+ pid(z_distance, 0.01);
+ pid(orientation,0.2);
+ pid(x_distance, 0.005);
+ // pid_yaw(orientation_x);
+ //si la distancia z (error) es menor a la minima distancia admitida (tolerancia)
+ if((fabs(this->orientation.error) < this->orientation.tolerance)) //No parar la rotación hasta que este cerca (25 cm de error)
+ {
+ ROS_INFO("CTrackingModule: Stop rotating");
+ orientation.pid_out=0.0;
+ }
+ if(this->z_distance.error < this->z_distance.tolerance)
+ {
+ ROS_INFO("CTrackingModule: Stop walking Z");
+ z_distance.pid_out=0.0;
+ }
+ if((fabs(this->x_distance.error)>this->x_distance.tolerance) && z_distance.error>0.3) //si la distancia x es mayor a la tolerancia
+ {
+ ROS_INFO("CTrackingModule: Rotate for X");
+ pid_yaw(orientation); //mover yaw dependiendo de distancia x
+ // x_distance.pid_out=0.0;
+ }
+ if(z_distance.pid_out==0.0)
+ {
+ ROS_INFO("CTrackingModule: Stop aproaching");
+
+ ROS_WARN("X: %f cm", (x_distance.qr_position)*100); //Distancia X QR
+ ROS_WARN("Y: %f cm", (z_distance.qr_position)*100); //Distancia Z QR
+ ROS_WARN("YAW: %f", (orientation.qr_position)*100);
+ orientation.distance_target=this->yaw_target;
+ this->state=ORIENTATE;
+ //this->status=TRACK_MODULE_SUCCESS;
+ }
+ else
+ this->state=TRACK_MODULE_TRACK;
+
+ ROS_WARN("Error YAW: %f, PID: %f", orientation.error, orientation.pid_out);
+ ROS_WARN("Error Z: %f, PID: %f", z_distance.error, z_distance.pid_out);
+ ROS_INFO("Error X: %f, PID: %f", x_distance.error, x_distance.pid_out);
+
+ walk_module.set_steps_size(z_distance.pid_out,0.0,orientation.pid_out);
+
+ // check if walk module failed
+ if(walk_module.is_finished())
+ {
+ ROS_INFO("CTrackingModule: Walk module failed!");
+ this->state=TRACK_MODULE_IDLE;
+ this->status=TRACK_MODULE_WALK_ERROR;
+ }
+ // check if head_tracking module failed
+ if(head_tracking_module.is_finished())
+ {
+ ROS_INFO("CTrackingModule: Head tracking module failed!");
+ this->state=TRACK_MODULE_IDLE;
+ this->status=TRACK_MODULE_HEAD_ERROR;
+ }
+ // check if qr was lost
+ if(this->watchdog_qr_lost.is_active())
+ {
+ ROS_ERROR("CTrackingModule: QR lost!");
+ walk_module.stop(); //stop walking
+ this->state=TRACK_MODULE_END;
+ this->status=TRACK_MODULE_LOST;
+ }
+
+ break;
+
+ case ORIENTATE: ROS_INFO("CTrackingModule: state ORIENTATE");
+ pid(orientation,0.1);
+ ROS_INFO("error: %f, qr_position: %f", orientation.error, orientation.qr_position);
+ if((fabs(this->orientation.error) < this->orientation.tolerance)) //No parar la rotación hasta que este cerca (25 cm de error)
+ {
+ ROS_INFO("CTrackingModule: Stop rotating");
+ orientation.pid_out=0.0;
+ walk_module.stop();
+ this->state=TRACK_MODULE_END;
+ }
+ else
+ {
+ walk_module.set_steps_size(0.0,0.0,orientation.pid_out);
+ this->state=ORIENTATE;
+ }
break;
case TRACK_MODULE_END: ROS_INFO("CTrackingModule: state END");
//this->tracking=false;
+ this->status=TRACK_MODULE_SUCCESS;
head_tracking_module.stop_tracking();
if(head_tracking_module.is_finished() && walk_module.is_finished())
this->state=TRACK_MODULE_IDLE;
@@ -378,7 +472,9 @@ void CTrackingModule::start_tracking(std::string qr_id, double z_target, double
/* set desired distance */
z_distance.distance_target=z_target;
x_distance.distance_target=x_target;
- orientation.distance_target=yaw_target;
+ //orientation.distance_target=yaw_target;
+ this->yaw_target=yaw_target;
+ orientation.distance_target=0.0;
/* set desired tolerance */
z_distance.tolerance=z_tolerance;
x_distance.tolerance=x_tolerance;
@@ -553,7 +649,7 @@ void CTrackingModule::qr_pose_callback(const humanoid_common_msgs::tag_pose_arra
if(this->qr_id==msg->tags[0].tag_id)
{
- qr_detected=true;
+ // qr_detected=true;
/* update pan and tilt*/
this->pan_angle=this->current_pan_angle+atan2(msg->tags[0].position.x,msg->tags[0].position.z);
@@ -604,6 +700,7 @@ void CTrackingModule::qr_pose_callback(const humanoid_common_msgs::tag_pose_arra
ROS_INFO("YAW: %f cm", (orientation.qr_position)*100);
}
+ qr_detected=true;
}
}else{