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();
	}
Exemple #3
0
	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;
		}