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; }