예제 #1
0
shared_ptr<action::Action> FixedAngleTrace::control(const string& taskName)
{
	float simTime = WM.getSimTime();
	LOG_PRINTF("task","current time: %f", simTime);
	
	//cout<<"TaskName:  "<<taskName<<endl;
	
	if( taskName!=mTaskName && true==mTask->changeable )
	{
		mEntryTaskName=taskName;
		setCurrentTask(taskName);
		LOG_PRINT("task"," start new task "+mTaskName);
		LOG_PRINT("task","current pos is "+getCurrentPose());
		LOG_FLUSH;

		//return Timing::control(WM.predictedPerception(), mDesiredPose, mTask->time);
		return Timing::control(WM.lastPerception(), mDesiredPose, mTask->time,taskName); //TT 1
	}

	float remainTime = mTaskBeginTime + mTask->time - simTime;
	if ( remainTime > 0.005f )
	{
		LOG_PRINT("task"," continue task "+mTaskName);
		LOG_PRINT("task","current pos is "+getCurrentPose());
		LOG_FLUSH;
		shared_ptr<action::Action> act
			//= Timing::control(WM.predictedPerception(), mDesiredPose, remainTime);
			= Timing::control(WM.lastPerception(), mDesiredPose, remainTime,taskName); //TT 2
		int adjustFoot = mTask->adjustFoot;
		if ( mFootExchanged ) adjustFoot = -adjustFoot;
		// FOOT_ADJUSTER.adjust( adjustFoot, shared_static_cast<action::JointAction>(act) );
		mIsFinished = false;
		return act;
	}

	// the task should be finished
	//// if no next task
	if ( "null" == mTask->next )
	{
		//cout<<" stop at task "<<mTaskName<<' '<<mTask->next<<'\n';
		mIsFinished = true;
		mTaskName = "null";
		//mEntryTaskName = "null";
		LOG_PRINTF("task","do null");
		LOG_FLUSH;

		//return Timing::control(WM.predictedPerception(),mDesiredPose,(20.0f * serversetting::sim_step));
		return Timing::control(WM.lastPerception(),mDesiredPose,(20.0f * serversetting::sim_step),taskName); //TT 3
	}

	//// start next task
	setCurrentTask( mTask->next );
	mIsStartNextTask = true;
	LOG_PRINT("task"," start next task "+mTaskName);
	LOG_FLUSH;
	
	//return Timing::control(WM.predictedPerception(), mDesiredPose, mTask->time);
	return Timing::control(WM.lastPerception(), mDesiredPose, mTask->time,taskName); //TT 4
}
void GoToSelectedBall::publishPose(float dist_from_ball){

	ROS_INFO("enter publishPose, distance = %f", dist_from_ball);

	if(isBallPoseSet== false){
		ROS_INFO("publishPose, wait for ball position, return");
		return;
	}

	float ball_odom_x = getCurrentPose().x;
	float ball_odom_y = getCurrentPose().y;


	float robot_odom_x, robot_odom_y;
	getRobotPositionInOdom(robot_odom_x, robot_odom_y);


	float dx = ball_odom_x - robot_odom_x;
	float dy = ball_odom_y - robot_odom_y;

	float dist = sqrt(pow(ball_odom_x - robot_odom_x, 2) + pow(ball_odom_y-robot_odom_y, 2) );

	ROS_INFO("dist = %f",dist);

	float goal_odom_x = robot_odom_x + dx * (dist - dist_from_ball) / dist;
	float goal_odom_y = robot_odom_y + dy * (dist - dist_from_ball) / dist;

	float angle = getAngle(robot_odom_x, robot_odom_y, ball_odom_x, ball_odom_y);

	tf::Quaternion q;
	float goal_map_x, goal_map_y;
	transFromOdomToMapPosition(goal_odom_x, goal_odom_y, angle, goal_map_x, goal_map_y, q);



	geometry_msgs::Quaternion qMsg;
	tf::quaternionTFToMsg(q, qMsg);


	move_base_msgs::MoveBaseGoal goal;

	goal.target_pose.pose.position.x = goal_map_x;
	goal.target_pose.pose.position.y = goal_map_y;
	goal.target_pose.pose.position.z = 0;


	goal.target_pose.pose.orientation = qMsg;

	goal.target_pose.header.stamp = ros::Time::now();
	goal.target_pose.header.frame_id ="/map";


	ROS_INFO("Sending goal");
	ac.sendGoal(goal);
	firstGoalSent = true;

}
float GoToSelectedBall::getAngleDiff(){

	float robot_odom_x, robot_odom_y;
	getRobotPositionInOdom(robot_odom_x, robot_odom_y);

	float goalAngle = getAngle(robot_odom_x, robot_odom_y, getCurrentPose().x, getCurrentPose().y);
	float robotAngle = getRobotAngleInOdom();

	return fabs(goalAngle-robotAngle);

}
예제 #4
0
	void BoxGrid::renderAnchor()
	{
		glColor4f(1.0,0,0,1.0);
		glBegin(GL_QUADS);
		for(int i=0;i<anchors.size();i++)
		{
			int idBox = anchors[i];
			RowVector3 p[8];
			p[0] = getCurrentPose(getBoxNodes(idBox,7));
			p[1] = getCurrentPose(getBoxNodes(idBox,6));
			p[2] = getCurrentPose(getBoxNodes(idBox,5));
			p[3] = getCurrentPose(getBoxNodes(idBox,4));
			p[4] = getCurrentPose(getBoxNodes(idBox,3));
			p[5] = getCurrentPose(getBoxNodes(idBox,2));
			p[6] = getCurrentPose(getBoxNodes(idBox,1));
			p[7] = getCurrentPose(getBoxNodes(idBox,0));
			glNormal((p[3]-p[7]).cross(p[6]-p[7]));//bottom
			glVertex(p[7]);
			glVertex(p[3]);
			glVertex(p[2]);
			glVertex(p[6]);
			glNormal((p[4]-p[5]).cross(p[1]-p[5]));//top
			glVertex(p[5]);
			glVertex(p[4]);
			glVertex(p[0]);
			glVertex(p[1]);
			glNormal((p[5]-p[7]).cross(p[3]-p[7]));//far
			glVertex(p[7]);
			glVertex(p[5]);
			glVertex(p[1]);
			glVertex(p[3]);
			glNormal((p[1]-p[3]).cross(p[2]-p[3]));//right
			glVertex(p[3]); 
			glVertex(p[1]);
			glVertex(p[0]);
			glVertex(p[2]);
			glNormal((p[2]-p[6]).cross(p[4]-p[6]));//front
			glVertex(p[6]);
			glVertex(p[2]);
			glVertex(p[0]);
			glVertex(p[4]);
			glNormal((p[6]-p[7]).cross(p[5]-p[7]));//left
			glVertex(p[7]);
			glVertex(p[6]);
			glVertex(p[4]);
			glVertex(p[5]);
		}
		glEnd();
		RowVector4 color(1.0,0.0,0.0,1.0);
		for(int i=0;i<anchorNodes.size();i++)
		{
			RowVector3 p = getCurrentPose(anchorNodes[i]);
			paintPoint(p, 0.007,color);
		}
	}
void GoToSelectedBall::publishAngle(){

	ROS_INFO("enter publishAngle");

	if(isBallPoseSet== false){
		ROS_INFO("publishAngle, wait for ball position, return");
		return;
	}


	float ball_odom_x = getCurrentPose().x;
	float ball_odom_y = getCurrentPose().y;

	float robot_odom_x, robot_odom_y;
	getRobotPositionInOdom(robot_odom_x, robot_odom_y);

	float goal_odom_x = robot_odom_x;// + dx * (dist - dist_from_ball) / dist;
	float goal_odom_y = robot_odom_y;// + dy * (dist - dist_from_ball) / dist;

	float angle = getAngle(robot_odom_x, robot_odom_y, ball_odom_x, ball_odom_y);

//tf::Quaternion q;
//float goal_map_x, goal_map_y;
//transFromOdomToMapPosition(goal_odom_x, goal_odom_y, angle, goal_map_x, goal_map_y, q);

	geometry_msgs::Quaternion qMsg;
//	tf::quaternionTFToMsg(q, qMsg);
	setAngle(angle, qMsg);

	move_base_msgs::MoveBaseGoal goal;

	goal.target_pose.pose.position.x = goal_odom_x;
	goal.target_pose.pose.position.y = goal_odom_y;
	goal.target_pose.pose.position.z = 0;


	goal.target_pose.pose.orientation = qMsg;

	goal.target_pose.header.stamp = ros::Time::now();
	goal.target_pose.header.frame_id ="/odom";


	ROS_INFO("Sending goal");
	ac.sendGoal(goal);
	firstGoalSent = true;
//	isBallPoseSet = false;

	ROS_INFO("return publishAngle");
}
예제 #6
0
	void BoxGrid::computeBoxPositions() {
		m_boxPositions.setZero(getNumBoxes(),3);
		#pragma omp parallel for
		for(int i = 0 ; i < getNumBoxes() ; i++) {
			for(int k = 0 ; k < 8 ; ++k) {
				m_boxPositions.row(i) += getCurrentPose(m_boxNodes(i,k));
			}
		}
		m_boxPositions *= 0.125;
	}
예제 #7
0
	//zhiping luo
	void BoxGrid::computeTransformation(NodeHandles & m_nodehandles)
	{
		vector<int> nodes;
		for(int idBox = 0; idBox <nnzBoxes; idBox++)
		{
			if(isHull(idBox))
			{
				for(int i = 0; i< 8; i++){
					int idNode = getBoxNodes(idBox, i);
					if(!inTheList(nodes, idNode)){
						nodes.push_back(idNode);
				    	RowVector3 P = getCurrentPose(idNode);
						RowVector3 P_rest = getRestPose(idNode);
						int m_numEdges = 0;
						Quaternion quaternions[6];
						for(int j = 0; j<6;j++){
							int idNeighborNode = getNodeNodes(idNode, j);
							if(idNeighborNode!=-1){
								float tmp[4];
								RowVector3 P1 = getRestPose(idNeighborNode);
								RowVector3 P2 = getCurrentPose(idNeighborNode);
								RowVector3 P3 = (P_rest - P1).normalized();
								RowVector3 P4 = (P - P2).normalized();
								extract_rotation_pseudo_edge(tmp, P3[0],P3[1],P3[2],P4[0],P4[1],P4[2]);
								Quaternion r(tmp[3],tmp[0],tmp[1],tmp[2]);
								r.normalize();
								quaternions[m_numEdges] = r;
								m_numEdges++;
							}
						}
						Quaternion r = AccurateAverageQuaternion(quaternions,m_numEdges);
						m_nodehandles.setR(idNode, r);
						m_nodehandles.setT(idNode, P);
					}
				}
			}
		}
		nodes.clear();
	}
void GoToSelectedBall::goToBall(){

	
	move_base_msgs::MoveBaseGoal goal;

	goal.target_pose.pose.position.x = getCurrentPose().x;
	goal.target_pose.pose.position.y = getCurrentPose().y;
	
	geometry_msgs::Quaternion qMsg;
	setAngle(getRobotAngleInMap(), qMsg);
	
	
	
	goal.target_pose.pose.orientation = qMsg;

	goal.target_pose.header.stamp = ros::Time::now();
	goal.target_pose.header.frame_id ="/odom";

	ROS_INFO("Sending goal...");
	ac.sendGoalAndWait(goal,ros::Duration(120),ros::Duration(0.1));
	firstGoalSent = true;

}
예제 #9
0
	const RowVector3 & BoxGrid::V(int idV) const {
		return getCurrentPose(idV);
	}
예제 #10
0
	const RowVector3 & BoxGrid::V(int idF, int idV) const {
		return getCurrentPose(faces(idF,idV));
	}
void MoveToSimpleServer::goalCallback(const MoveToSimpleGoalConstPtr& goal)
{
	ROS_INFO("Got a new goal to work on... Hmmm...");

	int driving_direction = 1;

	if(goal->driving_direction == MoveToSimpleGoal::REVERSE)
		driving_direction = -1;

	geometry_msgs::PoseStamped start_pose = getCurrentPose();

	geometry_msgs::PoseStamped current_pose;
	current_pose.header = start_pose.header;
	current_pose.pose = start_pose.pose;

	geometry_msgs::Point final_point;
	float angle;

	// Simple Transformation.
	// X = x.cos(theta) - y.sin(theta) + trans-x;
	// Y = x.sin(theta) + y.cos(theta) + trans-y;

	final_point.x = goal->goal_pose.pose.position.x * cos(2 * atan2(current_pose.pose.orientation.z, current_pose.pose.orientation.w)) -
					goal->goal_pose.pose.position.y * sin(2 * atan2(current_pose.pose.orientation.z, current_pose.pose.orientation.w)) +
					start_pose.pose.position.x;

	final_point.y = goal->goal_pose.pose.position.x * sin(2 * atan2(current_pose.pose.orientation.z, current_pose.pose.orientation.w)) +
					goal->goal_pose.pose.position.y * cos(2 * atan2(current_pose.pose.orientation.z, current_pose.pose.orientation.w)) +
					start_pose.pose.position.y;

	ros::Rate publish_rate(4);

	float xy_tolerance = goal->xy_tolerance > 0.075 ? goal->xy_tolerance : 0.075;

	while(!equals(final_point, current_pose.pose.position, xy_tolerance))
	{
		////////////////////////////
		///// Publish velocity /////
		////////////////////////////
		if(driving_direction == 1)
			angle = atan2(final_point.y - current_pose.pose.position.y, final_point.x - current_pose.pose.position.x);
		else
			angle = atan2(current_pose.pose.position.y - final_point.y, current_pose.pose.position.x - final_point.x);

		float error_fi = angle - (2 * atan2(current_pose.pose.orientation.z, current_pose.pose.orientation.w));

		geometry_msgs::Twist resultant_velocity;

		if( sqrt( (final_point.x - current_pose.pose.position.x) * (final_point.x - current_pose.pose.position.x) +
				   (final_point.y - current_pose.pose.position.y) * (final_point.y - current_pose.pose.position.y)) < 0.75)
			resultant_velocity.linear.x = 0.15 * driving_direction;
		else
			resultant_velocity.linear.x = 0.3 * driving_direction;

		resultant_velocity.angular.z = (atan2( sin(error_fi), cos(error_fi)) * 0.66);

		if(resultant_velocity.angular.z > 1.5)
			resultant_velocity.angular.z = 1.5;
		else if(resultant_velocity.angular.z < -1.5)
			resultant_velocity.angular.z = -1.5;

		_cmd_vel_pub.publish(resultant_velocity);

		//ROS_INFO("Velocities: x, theta = (%f, %f)", resultant_velocity.linear.x, resultant_velocity.angular.z);
		//ROS_INFO("ANGLE: (%f) and [%f]", angle, (2 * atan2(current_pose.pose.orientation.z, current_pose.pose.orientation.w)));

		////////////////////////////////////
		///// Recalculate our position /////
		////////////////////////////////////

		current_pose = getCurrentPose();

		publish_rate.sleep();
	}

	// Execute the rotation behaviour.
	float goal_fi_relative = 2*atan2(goal->goal_pose.pose.orientation.z, goal->goal_pose.pose.orientation.w);
	float goal_fi = 2*atan2(start_pose.pose.orientation.z, start_pose.pose.orientation.w) + goal_fi_relative;
	goal_fi = atan2(sin(goal_fi), cos(goal_fi));

	float our_fi = 2*atan2(current_pose.pose.orientation.z, current_pose.pose.orientation.w);

	float yaw_tolerance = goal->yaw_tolerance > 0.15 ? goal->yaw_tolerance : 0.15;;

	while(ros::ok())
	{
		current_pose = getCurrentPose();

		our_fi = 2*atan2(current_pose.pose.orientation.z, current_pose.pose.orientation.w);

		float del_fi = atan2(sin(goal_fi - our_fi), cos((goal_fi - our_fi)));

		geometry_msgs::Twist resultant_velocity;

		resultant_velocity.angular.z = (del_fi * 0.66);

		if(resultant_velocity.angular.z > 1.5)
			resultant_velocity.angular.z = 1.5;
		else if(resultant_velocity.angular.z < -1.5)
			resultant_velocity.angular.z = -1.5;

		_cmd_vel_pub.publish(resultant_velocity);

		//ROS_INFO("ANGLE: goal_relative: %f", goal_fi_relative);
		//ROS_INFO("del_fi: (%f)", del_fi);

		publish_rate.sleep();

		if(fabs(del_fi) < yaw_tolerance )
			break;
	}

	ROS_INFO("Action succeeded!");
	_server.setSucceeded();

}