void
HeadGestures::processNod()
{
if (vTrajectory_.size() > 0)
{
targetPosition tp = vTrajectory_.at(0);
if (tp.direction == TILT_UP)
{
if (DEBUG_)
std::cout << "Tilt Up: currTiltAngle_ = " << currTiltAngle_ << " tp.angle = " << tp.angle << " x = " << 10 * cos(tp.angle) << " z = " << 10 * sin(tp.angle) << std::endl;
if (currTiltAngle_ < (tp.angle - ANGLE_ERROR_MARGIN))
{
setHeadPosition("base_link", 10 * cos(tp.angle), 10 * sin(currPanAngle_), 10 * sin(tp.angle));
}
else
{
vTrajectory_.erase(vTrajectory_.begin());
}
}
else
if (tp.direction == TILT_DOWN)
{
if (DEBUG_)
std::cout << "Tilt Down: currPanAngle_ = " << currPanAngle_ << " tp.angle = " << tp.angle << " x = " << 10 * cos(tp.angle) << " z = " << 10 * sin(tp.angle) << std::endl;
if (currTiltAngle_ > tp.angle + ANGLE_ERROR_MARGIN)
{
setHeadPosition("base_link", 10 * cos(tp.angle), 10 * sin(currPanAngle_), 10 * sin(tp.angle));
}
else
{
vTrajectory_.erase(vTrajectory_.begin());
}
}
}
}
void
HeadGestures::processShake()
{
if (vTrajectory_.size() > 0)
{
targetPosition tp = vTrajectory_.at(0);
if (tp.direction == PAN_RIGHT)
{
if (DEBUG_)
std::cout << "Pan Right: currPanAngle_ = " << currPanAngle_ << " tp.angle = " << tp.angle << " x = " << 10 * cos(tp.angle) << " y = " << 10 * sin(tp.angle) << std::endl;
if (currPanAngle_ > (tp.angle + ANGLE_ERROR_MARGIN))
{
setHeadPosition("base_link", 10 * cos(tp.angle), 10 * sin(tp.angle), 0);
}
else
{
vTrajectory_.erase(vTrajectory_.begin());
}
}
else
if (tp.direction == PAN_LEFT)
{
if (DEBUG_)
std::cout << "Pan Left: currPanAngle_ = " << currPanAngle_ << " tp.angle = " << tp.angle << " x = " << 10 * cos(tp.angle) << " y = " << 10 * sin(tp.angle) << std::endl;
if (currPanAngle_ < tp.angle - ANGLE_ERROR_MARGIN)
{
setHeadPosition("base_link", 10 * cos(tp.angle), 10 * sin(tp.angle), 0);
}
else
{
vTrajectory_.erase(vTrajectory_.begin());
}
}
}
}
void StereoSelector::moveHead()
{
if(!move_head_bool_)
return;
float pan_velocity, tilt_velocity;
u_act_ = object_pos_.x; //The -20 is because we consider the left eye, thus we need a small offset
//to keep the head centered in respect to the object
v_act_ = object_pos_.y;
/*
* This threshold is used to avoid small movements of the head.
* Thus, if the object's position is near to the center of the image, then the head movement is zero
*/
float u_threshold = 3;
float v_threshold = 3;
if(abs(u_act_) < u_threshold)
u_act_ = 0;
else if(u_act_<-1*u_threshold)
u_act_ = u_act_ +u_threshold;
else if(u_act_>u_threshold)
u_act_ = u_act_-u_threshold;
if(abs(v_act_) < v_threshold)
v_act_ = 0;
else if(v_act_<-1*v_threshold)
v_act_ = v_act_ +v_threshold;
else if(v_act_>v_threshold)
v_act_ = v_act_-v_threshold;
diff_u_=u_act_-u_prev_;
pan_velocity=controlPID(u_act_,0,diff_u_,kp_u_,ki_u_,kd_u_);
u_prev_=u_act_;
diff_v_=v_act_-v_prev_;
tilt_velocity =controlPID(v_act_,0,diff_v_,kp_v_,ki_v_,kd_v_);
v_prev_=v_act_;
setHeadPosition(v_act_, u_act_, tilt_velocity, pan_velocity);
}
