コード例 #1
0
ファイル: move_right_bt.cpp プロジェクト: almc/nao_basic
	void initialize()
		{


			// write "right" to ltl.planner
			std_msgs::String msg;
			std::stringstream ss;
			ss << "right" << std::endl;
			std::cout << "**AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAGoToWaypoint -%- Executing Main Task, elapsed_time: " << std::endl;

			msg.data = ss.str();
			// ROS_INFO("%s", msg.data.c_str());
			chatter_pub.publish(msg);
			std::cout << "**msg:"
			          << msg << std::endl;
			counter++;

			sleep(1.0);
			//Set the stiffness so the robot can move
			AL::ALValue stiffness_name("Body");
			AL::ALValue stiffness(1.0f);
			AL::ALValue stiffness_time(1.0f);
			motion_proxy_ptr->stiffnessInterpolation(stiffness_name,
			                                         stiffness,
			                                         stiffness_time);
			if( message_.type == "goto")
				{
				 motion_proxy_ptr->moveInit();
				}
			set_feedback(RUNNING);
		}
コード例 #2
0
void NaoMarkServiceDetection::callback(const std::string &key, const AL::ALValue &value, const AL::ALValue &msg) 
{
	AL::ALValue marks = fMemoryProxy.getData("LandmarkDetected");

	if(marks.getSize() > 0 && !_isMarkFound)
	{
		int TimeStampField = marks[0][1];
		if((int)marks[1][0][1][0] == _markToFind)
		{
			if((float)marks[1][0][0][3] < 0.2f)
			{
				if(_isAllowedToMove)
					motionProxy->moveToward(0.5f,0,marks[1][0][0][1]);
				_naoMarkDetected = true;
			}
			else
			{
				motionProxy->moveToward(0,0,0);
				_isMarkFound = true;
			}
		}
		else
			motionProxy->moveToward(0,0,0);
	}
}
コード例 #3
0
	void initialize()
	{
		init_ = true;

		// Enable stiffness
		AL::ALValue stiffness_name("Body");
		AL::ALValue stiffness(1.0f);
		AL::ALValue stiffness_time(1.0f);
		motion_proxy_ptr->stiffnessInterpolation(stiffness_name,stiffness,stiffness_time);

		// Init moving
		motion_proxy_ptr->moveInit();

		// Robot not detected
		robotDetected = false;
	}
コード例 #4
0
	void finalize()
	{
		// Stop moving
		motion_proxy_ptr->stopMove();

		init_ = false;
		deactivate();
	}
コード例 #5
0
	void finalize()
	{
		// Stop rotating
		motion_proxy_ptr->stopMove();

		// Delete Filter
		delete ic;

		init_ = false;
		deactivate();
	}
コード例 #6
0
	void initialize()
	{
		init_ = true;

		// Enable stiffness
		AL::ALValue stiffness_name("Body");
		AL::ALValue stiffness(1.0f);
		AL::ALValue stiffness_time(1.0f);
		motion_proxy_ptr->stiffnessInterpolation(stiffness_name,stiffness,stiffness_time);

		// Stand
		//robotPosture->goToPosture("Stand",0.5f);

		// Init moving
		motion_proxy_ptr->moveInit();

		// Initial Odometry
		x_0 = motion_proxy_ptr->getRobotPosition(true).at(0);
		y_0 = motion_proxy_ptr->getRobotPosition(true).at(1);
		z_0 = motion_proxy_ptr->getRobotPosition(true).at(2);
	}
コード例 #7
0
void NaoMarkServiceDetection::backGroundService()
{ 
	_isThreadRuning = true;
	while(_isThreadRuning)
	{
		_naoMarkDetected = false;
		Sleep((DWORD)1000);
		if(!_naoMarkDetected && _markToFind != -1 && !_isMarkFound)
		{
			motionProxy->moveToward(0,0,0);
			FindMarkArround();
		}
	}
}
コード例 #8
0
	int executeCB(ros::Duration dt)
	{
		std::cout << "**Walk -%- Executing Main Task, elapsed_time: "
		          << dt.toSec() << std::endl;
		std::cout << "**Walk -%- execute_time: "
		          << execute_time_.toSec() << std::endl;
		execute_time_ += dt;

		if(!init_)
		{
			set_feedback(RUNNING);
			initialize();
		}

		double x = motion_proxy_ptr->getRobotPosition(true).at(0);
		double y = motion_proxy_ptr->getRobotPosition(true).at(1);
		double z = motion_proxy_ptr->getRobotPosition(true).at(2);

		// Walk forward
		double angular = 0.1*modulo2Pi(z_0-z);
		//ROS_INFO("theta = %f",angular);
		if(angular > 1) {angular = 1;}
		if(angular < -1) {angular = -1;}
		geometry_msgs::Twist cmd;
		cmd.linear.x = 0.3; //0.5
		cmd.angular.z = angular;
		cmd_pub.publish(cmd);

		if(sqrt((x-x_0)*(x-x_0) + (y-y_0)*(y-y_0)) > dist)
		{
			set_feedback(SUCCESS);
			finalize();
			return 1;
		}

		return 0;
	}
コード例 #9
0
ファイル: move_right_bt.cpp プロジェクト: almc/nao_basic
	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;
		}
コード例 #10
0
void NaoMarkServiceDetection::MoveHead(float angle)
{
	motionProxy->angleInterpolation("HeadYaw", AL::ALValue(angle), AL::ALValue(1.0f), true);
	motionProxy->stiffnessInterpolation("HeadYaw", 1.0f, 1.0f);
}
コード例 #11
0
void NaoMarkServiceDetection::TurnToDetectedMark(float angle)
{
	motionProxy->moveTo(0,0,angle);
	MoveHead(0);
	_isAllowedToMove = true;
}
コード例 #12
0
std::pair<float, float> worldBallPosFromImgCoords(AL::ALMotionProxy motionProxy,
                                                  std::pair<int, int> ballPosCam,
                                                  int imgWidth, int imgHeight,
                                                  int camera)
{
	std::string cameraName = "CameraTop";
	if (camera == 1)
	{
		cameraName = "CameraBottom";
	}

	// Image coordinates of ball
	int u = ballPosCam.first;
	int v = ballPosCam.second;

	// Angles of observation of the ball
	float phi = ((float)u-((float)imgWidth)/2)/(float)imgWidth * img_WFOV;
	float theta = ((float)v-((float)imgHeight)/2)/(float)imgHeight * img_HFOV;

	// Select the right coordinates for the NAO system!
	// x outward from camera, y to the left and z vertically upwards

	// Coefficients for line-equation going from NAO camera to the ball
	float b_c = -sin(phi);
	float c_c = -sin(theta);
	float a_c = sqrt((cos(phi)*cos(phi)+cos(theta)*cos(theta))/2);

	int space = 2; //FRAME_ROBOT
	bool useSensorValues = true;
	std::vector<float> transVec =
		motionProxy.getTransform(cameraName, space, useSensorValues); // Transform camera -> FRAME_ROBOT
	std::vector<float> cameraPos =
		motionProxy.getPosition(cameraName, space, useSensorValues); // Camera position in FRAME_ROBOT


	// std::cout << "Position of bottom camera: " << std::endl;
	// std::cout << cameraPos.at(0) << " " << cameraPos.at(1) << " " << cameraPos.at(2) << std::endl;
	// std::cout << cameraPos.at(3) << " " << cameraPos.at(4) << " " << cameraPos.at(5) << std::endl;


	// Put the camera transform into an Eigen matrix for easy multiplication
	Eigen::Matrix4f trans;
	trans <<
		transVec[0] , transVec[1] , transVec[2] , transVec[3] ,
		transVec[4] , transVec[5] , transVec[6] , transVec[7] ,
		transVec[8] , transVec[9] , transVec[10], transVec[11],
		transVec[12], transVec[13], transVec[14], transVec[15];

	Eigen::Vector4f vec(a_c, b_c, c_c, 1);

	// Transform the line equation from NAO camera coordinate system into FRAME_ROBOT
	Eigen::Vector4f transformedLine = trans*vec;
	// std::cout << "trans*vec = " << transformedLine << std::endl;

	// Solve line-plane intersection with plane at z (floor)
	// Solution from Wikipedia line-plane intersection article
	float z = 0.00;

	Eigen::Matrix3f lineMat;
	lineMat <<
		cameraPos.at(0)-transformedLine[0], 1.0-0.0, 0.0-0.0,
		cameraPos.at(1)-transformedLine[1], 0.0-0.0, 1.0-0.0,
		cameraPos.at(2)-transformedLine[2], z  -  z, z  -  z;

	Eigen::Vector3f lineVec;
	lineVec << cameraPos.at(0)-0.0, cameraPos.at(1)-0.0, cameraPos.at(2)-z;
	Eigen::Vector3f txy = lineMat.inverse()*lineVec;
	std::cout << "Ball is at (x, y): (" << txy[1] << ", " << txy[2] << ")" << std::endl;
	std::pair<float, float> return_value(txy[1], txy[2]);
	return return_value; //Return ball position (x, y)
}