void finalize()
{
// Stop moving
motion_proxy_ptr->stopMove();
init_ = false;
deactivate();
}
void finalize()
{
// Stop rotating
motion_proxy_ptr->stopMove();
// Delete Filter
delete ic;
init_ = false;
deactivate();
}
int executeCB(ros::Duration dt)
{
std::cout << "**GoToWaypoint -%- Executing Main Task, elapsed_time: "
<< dt.toSec() << std::endl;
std::cout << "**GoToWaypoint -%- execute_time: "
<< execute_time_.toSec() << std::endl;
execute_time_ += dt;
std::cout << "**Counter value:" << counter << std::endl;
if (counter > 1)
std::cout << "********************************************************************************" << std::endl;
if (!init_)
{
initialize();
init_ = true;
}
if ( (ros::Time::now() - time_at_pos_).toSec() < 0.2 )
{
if( message_.type == "goto")
{
// Goal position of ball relative to ROBOT_FRAME
float goal_x = 0.00;
float goal_y = 0.00;
float error_x = message_.x - goal_x;
float error_y = message_.y - goal_y;
if (fabs(error_x) < 0.12 && fabs(error_y) < 0.12)
{
std::cout << "Closeness count " << closeness_count << std::endl;
closeness_count++;
//If the NAO has been close for enough iterations, we consider to goal reached
if (closeness_count > 0)
{
motion_proxy_ptr->stopMove();
set_feedback(SUCCESS);
// std::cout << "sleeeping for 2 second before destroying thread" << std::endl;
// sleep(2.0);
finalize();
return 1;
}
}
else
{
closeness_count = 0;
}
//Limit the "error" in order to limit the walk speed
error_x = error_x > 0.6 ? 0.6 : error_x;
error_x = error_x < -0.6 ? -0.6 : error_x;
error_y = error_y > 0.6 ? 0.6 : error_y;
error_y = error_y < -0.6 ? -0.6 : error_y;
// float speed_x = error_x * 1.0/(2+5*closeness_count);
// float speed_y = error_y * 1.0/(2+5*closeness_count);
// float frequency = 0.1/(5*closeness_count+(1.0/(fabs(error_x)+fabs(error_y)))); //Frequency of foot steps
// motion_proxy_ptr->setWalkTargetVelocity(speed_x, speed_y, 0.0, frequency);
// ALMotionProxy::setWalkTargetVelocity(const float& x, const float& y, const float& theta, const float& frequency)
AL::ALValue walk_config;
//walk_config.arrayPush(AL::ALValue::array("MaxStepFrequency", frequency));
//Lower value of step height gives smoother walking
// std::cout << "y " << message_.y << std::endl;
if (fabs(message_.y) < 0.10)
{
// walk_config.arrayPush(AL::ALValue::array("StepHeight", 0.01));
// motion_proxy_ptr->post.moveTo(message_.x, 0.0, 0.0, walk_config);
motion_proxy_ptr->post.moveTo(message_.x, 0.0, 0.0);
sleep(2.0);
//motion_proxy_ptr->post.stopMove();
}
else
{
// walk_config.arrayPush(AL::ALValue::array("StepHeight", 0.005));
// motion_proxy_ptr->post.moveTo(0.0, 0.0, message_.y/fabs(message_.y)*0.1, walk_config);
motion_proxy_ptr->post.moveTo(0.0, 0.0, message_.y/fabs(message_.y)*0.1);
//sleep(3.0);
//motion_proxy_ptr->post.stopMove();
}
}
}
set_feedback(RUNNING);
return 0;
}
