C++ QVec::print方法代码示例

void SpecificWorker::action_ChangeRoom(bool newAction)
{
	static float lastX = std::numeric_limits<float>::quiet_NaN();
	static float lastZ = std::numeric_limits<float>::quiet_NaN();

	auto symbols = worldModel->getSymbolsMap(params, "r2", "robot");


	try
	{
		printf("trying to get _in_ edge from %d to %d\n", symbols["robot"]->identifier, symbols["r2"]->identifier);
		AGMModelEdge e = worldModel->getEdge(symbols["robot"], symbols["r2"], "in");
		printf("STOP! WE ALREADY GOT THERE!\n");
		stop();
		return;
	}
	catch(...)
	{
	}

	int32_t roomId = symbols["r2"]->identifier;
	printf("room symbol: %d\n",  roomId);
	std::string imName = symbols["r2"]->getAttribute("imName");
	printf("imName: <%s>\n", imName.c_str());

	const float refX = str2float(symbols["r2"]->getAttribute("x"));
	const float refZ = str2float(symbols["r2"]->getAttribute("z"));

	QVec roomPose = innerModel->transformS("world", QVec::vec3(refX, 0, refZ), imName);
	roomPose.print("goal pose");
	// 	AGMModelSymbol::SPtr goalRoom = worldModel->getSymbol(str2int(params["r2"].value));
	// 	const float x = str2float(goalRoom->getAttribute("tx"));
	// 	const float z = str2float(goalRoom->getAttribute("tz"));
	const float x = roomPose(0);
	const float z = roomPose(2);

	bool proceed = true;
	if ( (planningState.state=="PLANNING" or planningState.state=="EXECUTING") )
	{
		if (abs(lastX-x)<10 and abs(lastZ-z)<10)
			proceed = false;
		else
			printf("proceed because the coordinates differ (%f, %f), (%f, %f)\n", x, z, lastX, lastZ);
	}
	else
	{
		printf("proceed because it's stoped\n");
	}

	if (proceed)
	{
		lastX = x;
		lastZ = z;
		printf("changeroom to %d\n", symbols["r2"]->identifier);
		go(x, z, 0, false, 0, 0, 1200);
	}
	else
	{
	}
}

///UNFiNISHED
bool Sampler::searchRobotValidStateCloseToTarget(QVec& target)
{
	//If current is good, return
	if( std::get<bool>(checkRobotValidStateAtTarget(target,QVec::zeros(3))) == true)
		return true;
	
	target.print("target");
	//Start searching radially from target to origin and adding the vertices of a n regular polygon of radius 1000 and center "target"
	const int nVertices = 12;
	const float radius = 1000.f;
	QVec lastPoint, minVertex, vertex;
	float fi,vert;
	float minDist = radius;
	
	for(int i=0; i< nVertices; i++)
	{
		fi = (2.f*M_PI/nVertices) * i;
		int k;
		bool free;
		for(k=100; k<radius; k=k+100)
		{
			vertex = QVec::vec3(target.x() + k*sin(fi), target.y(), target.z() + k*cos(fi));
			free = std::get<bool>(checkRobotValidStateAtTarget(vertex, QVec::zeros(3)));
			if (free == true) 
				break;
		}
		if( free and k < minDist )
		{
			minVertex = vertex;
			minDist = k;	
			vert = fi;
		}
	}
	if( minDist < radius)
	{
		target = minVertex;
		target.print("new target");
		qDebug() << minDist << vert;
		return true;
	}
	else
		return false;
}

void SpecificWorker::action_ReachPose(bool newAction)
{
	printf("action_ReachPose,%d: %d\n", __LINE__, newAction);
	static float lastX = std::numeric_limits<float>::quiet_NaN();
	printf("action_ReachPose,%d: %d\n", __LINE__, newAction);
	static float lastZ = std::numeric_limits<float>::quiet_NaN();
	printf("action_ReachPose,%d: %d\n", __LINE__, newAction);

	auto symbols = worldModel->getSymbolsMap(params, "room", "pose");
	printf("action_ReachPose,%d: %d\n", __LINE__, newAction);

	int32_t poseId = symbols["pose"]->identifier;
	printf("pose symbol: %d\n",  poseId);
	std::string imName = symbols["pose"]->getAttribute("imName");
	printf("imName: <%s>\n", imName.c_str());

	QVec pose = innerModel->transform6D("world", QString::fromStdString(imName));
	pose.print("goal pose");
	const float x = pose(0);
	const float z = pose(2);
	const float ry = pose(4);

	bool proceed = true;
	if ( (planningState.state=="PLANNING" or planningState.state=="EXECUTING") )
	{
		if (abs(lastX-x)<10 and abs(lastZ-z)<10)
			proceed = false;
		else
			printf("proceed because the coordinates differ (%f, %f), (%f, %f)\n", x, z, lastX, lastZ);
	}
	else
	{
		printf("proceed because it's stoped\n");
	}

	if (proceed)
	{
		lastX = x;
		lastZ = z;
		printf("setpose %d\n", symbols["room"]->identifier);
		go(x, z, ry, true);
	}
	else
	{
	}
}

void Worker::compute()
{
	/// Clear laser measurement
	for (int32_t i=0; i<LASER_SIZE; ++i)
	{
		(*laserDataW)[i].dist = maxLength;
	}


	/// Update InnerModel with joint information
	if (updateJoint)
	{
		RoboCompJointMotor::MotorStateMap motorMap;
		try
		{
			jointmotor->getAllMotorState(motorMap);
			for (RoboCompJointMotor::MotorStateMap::iterator it=motorMap.begin(); it!=motorMap.end(); ++it)
			{
				innerModel->updateJointValue(it->first.c_str(), it->second.pos);
			}
		}
		catch (const Ice::Exception &ex)
		{
			cout << "Can't connect to jointMotor: " << ex << endl;
		}
	}
	else
	{
		printf("not using joint\n");
	}

	/// FOR EACH OF THE CONFIGURED PROXIES
// 	#pragma omp parallel for
	for (uint r=0; r<rgbds.size(); ++r)
	{
#ifdef STORE_POINTCLOUDS_AND_EXIT
		pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>); // PCL
#endif

		if (rgbds[r].bus == true) /// If the proxy is a bus
		{
			/// FOR EACH OF THE CAMERAS OF THE BUS
			RoboCompRGBDBus::CameraList clist;
			clist.resize(1);
			RoboCompRGBDBus::CameraParamsMap::iterator iter;
			for (iter=rgbds[r].cameras.begin(); iter!=rgbds[r].cameras.end(); iter++)
			{
				if (iter->first == rgbds[r].id)
				{
					clist[0] = iter->first;
					RoboCompRGBDBus::ImageMap images;

					try
					{
						if (DECIMATION_LEVEL == 0)
							rgbds[r].proxyRGBDBus->getImages(clist,images);
						else
							rgbds[r].proxyRGBDBus->getDecimatedImages(clist, DECIMATION_LEVEL, images);
					}
					catch (const Ice::Exception &ex)
					{
						cout << "Can't connect to rgbd: " << ex << endl;
						continue;
					}

					/// Get the corresponding (stored) protocloud
					RoboCompRGBDBus::PointCloud pointCloud = rgbds[r].protoPointClouds[clist[0]];
					/// Multiply the protocloud by the depth
					for (uint32_t pi=0; pi<pointCloud.size(); pi++)
					{
						pointCloud[pi].x *= images[iter->first].depthImage[pi];
						pointCloud[pi].y *= images[iter->first].depthImage[pi];
						pointCloud[pi].z  = images[iter->first].depthImage[pi];
					}

					/// Inserts the resulting points in the virtual laser
					RTMat TR = innerModel->getTransformationMatrix(base, QString::fromStdString(iter->first));
#ifdef STORE_POINTCLOUDS_AND_EXIT
					cloud->points.resize(pointCloud.size());
#endif
					for (uint32_t ioi=0; ioi<pointCloud.size(); ioi+=3)
					{
						if ((not isnan(images[iter->first].depthImage[ioi])) and images[iter->first].depthImage[ioi] > 10)
						{
							QVec p = (TR * QVec::vec4(pointCloud[ioi].x, pointCloud[ioi].y, pointCloud[ioi].z, 1)).fromHomogeneousCoordinates();
#ifdef STORE_POINTCLOUDS_AND_EXIT
							cloud->points[ioi].x =  p(0)/1000;
							cloud->points[ioi].y =  p(1)/1000;
							cloud->points[ioi].z = -p(2)/1000;
#endif
							if ( (p(1)>=minHeight and p(1)<=maxHeight) /* or (p(1)<minHeightNeg) */)
							{
								p(1) = 0;
								float d = sqrt(p(0)*p(0) + p(2)*p(2));
								if (d>maxLength) d = maxLength;
								const float a = atan2(p(0), p(2));
								const int32_t bin = angle2bin(a);
								if (bin>=0 and bin<LASER_SIZE and (*laserDataW)[bin].dist > d)
								{
									if ( (*laserDataW)[bin].dist > d)
//.........这里部分代码省略.........

/**
*  \brief Called when the robot is sent close to an object's location
*/
void SpecificWorker::action_SetObjectReach(bool newAction)
{	// Get symbols' map
	std::map<std::string, AGMModelSymbol::SPtr> symbols;
	try
	{
		symbols = worldModel->getSymbolsMap(params); //ALL THE SYMBOLS GIVEN IN THE RULE
	}
	catch(...)
	{
		printf("navigationAgent: Couldn't retrieve action's parameters\n");
		printf("<<WORLD\n");
		AGMModelPrinter::printWorld(worldModel);
		printf("WORLD>>\n");
		if (worldModel->size() > 0) { exit(-1); }
	}

	// Get target
	int roomID, objectID, robotID;
	try
	{
		if (symbols["room"].get() and symbols["object"].get() and symbols["robot"].get() )
		{
			roomID = symbols["room"]->identifier;
			objectID =symbols["object"]->identifier;
			robotID = symbols["robot"]->identifier;

			try // If we can access the 'reach' edge for the object of the action
			{   // is not really necessary. The planner is probably replanning.
				worldModel->getEdgeByIdentifiers(objectID, objectID, "reach");
				{
					static QTime lastMsg = QTime::currentTime().addSecs(-1000);
					if (lastMsg.elapsed() > 1000)
					{
						rDebug2(("navigationAgent ignoring action setObjectReach (object currently reached)"));
						lastMsg = QTime::currentTime();
						return;
					}
					printf("ask the platform to stop\n");
					stop();
				}
			}
			catch(...)
			{
			}

		}
		else
		{
			printf("parameters not in the model yet\n");
			return;
		}
	}
	catch(...)
	{
		printf("ERROR: SYMBOL DOESN'T EXIST \n");
		exit(2);
	}

	// GET THE INNERMODEL NAMES OF TH SYMBOLS
	QString robotIMID;
	QString roomIMID;
	QString objectIMID;
	try
	{
		robotIMID = QString::fromStdString(worldModel->getSymbol(robotID)->getAttribute("imName"));
		roomIMID = QString::fromStdString(worldModel->getSymbol(roomID)->getAttribute("imName"));
		objectIMID = QString::fromStdString(worldModel->getSymbol(objectID)->getAttribute("imName"));

		// check if object has reachPosition
		AGMModelSymbol::SPtr object = worldModel->getSymbol(objectID);
		for (auto edge = object->edgesBegin(worldModel); edge != object->edgesEnd(worldModel); edge++)
		{
			if (edge->getLabel() == "reachPosition")
			{
				const std::pair<int32_t, int32_t> symbolPair = edge->getSymbolPair();
				objectID = symbolPair.second;
				objectIMID = QString::fromStdString(worldModel->getSymbol(objectID)->getAttribute("imName"));
				qDebug() << __FUNCTION__ << "Target object " << symbolPair.first<<"->"<<symbolPair.second<<" object "<<objectIMID;
			}
		}
	}
	catch(...)
	{
		printf("ERROR IN GET THE INNERMODEL NAMES\n");
		exit(2);
	}

	// GET THE TARGET POSE:
	RoboCompTrajectoryRobot2D::TargetPose tgt;
	try
	{
		if (not (innerModel->getNode(roomIMID) and innerModel->getNode(objectIMID)))
		{
			printf("Cant find objects to reach %d\n", __LINE__);
			return;
		}
		QVec poseInRoom = innerModel->transform6D(roomIMID, objectIMID); // FROM OBJECT TO ROOM
//.........这里部分代码省略.........

void SpecificWorker::ballTouched(bool first)
{
	QTime fff;
	if (first) fff = QTime::currentTime();

// 	if (tocaButton->isChecked())
	{
		tocaButton->setText("tocando");
		const int32_t ball = atoi(params["b"].value.c_str());
		const int32_t robot = atoi(params["r"].value.c_str());

		printf("\n\n----------\n");
		try
		{
			// Get 
			const float tx = str2float(worldModel->getSymbol(ball)->getAttribute("tx"));
			const float ty = str2float(worldModel->getSymbol(ball)->getAttribute("ty"));
			const float tz = str2float(worldModel->getSymbol(ball)->getAttribute("tz"));
			const QVec offset = QVec::vec3(0., 0., -100.);
			const QVec targetRobot = QVec::vec3(tx, ty, tz) + offset;
			
			const QVec actualWristPosition = QVec::vec3(
			str2float(worldModel->getSymbol(robot)->attributes["rightwrist_tx"]),
			str2float(worldModel->getSymbol(robot)->attributes["rightwrist_ty"]),
			str2float(worldModel->getSymbol(robot)->attributes["rightwrist_tz"]));
			
			actualWristPosition.print("actualWristPosition");
			

			const QVec error = targetRobot-actualWristPosition;

			// If the hand is far from the target, move the hand
			int32_t extra = 0.015 * (fff.elapsed() - 2000);
			if (extra<0) extra = 0;
			if (extra>200) extra = 200;
			int32_t umbral = 100. + extra;
			if (error.norm2() > umbral)
			{
				targetRobot.print("targetRobot");
				actualWristPosition.print("actualWristPosition");
				error.print("error");
				printf("ERROR: %f\n", error.norm2());
				printf("UMBRAL: %d\n", umbral);
				const QVec poseTr = innerModel->transform("world", targetRobot, "robot");
				printf("move hand to T=(%.2f, %.2f, %.2f)  \n", poseTr(0), poseTr(1), poseTr(2));
				sendRightHandPose(poseTr, QVec::vec3(0,0,0), QVec::vec3(1,1,1), QVec::vec3(0,0,0));		
			}
			// If the hand is close to the target, acknowledge the new state
			else
			{
				printf("lo conseguimos!!!\n");
				printf("lo conseguimos!!!\n");
				printf("lo conseguimos!!!\n");
				printf("lo conseguimos!!!\n");
				printf("lo conseguimos!!!\n");
				printf("lo conseguimos!!!\n");
				resetGame();
			}
		}
		catch(AGMModelException &e)
		{
			printf("I don't know ball %d\n", ball);
		}
	}
}