Shortest distance from cyllinder 30008 to a Tag is 126.572 to tag 11
Shortest distance from cyllinder 30012 to a Tag is 136.132 to tag 10
Shortest distance from cyllinder 30023 to a Tag is 107.067 to tag 10
Shortest distance from cyllinder 30031 to a Tag is 560.32 to tag 10

						cout << "got an identified cylinder!" << endl;
						cout << "coordX = " << myCylinder->getCentroid().coordX() << ", coordY = " << myCylinder->getCentroid().coordY() << endl;
						targetX = myCylinder->getCentroid().coordX();
						targetY = myCylinder->getCentroid().coordY();
						graspreq.object = myCylinder;
						targetAcquired = true;

myAprilTag->getCentroid() = c:[396.646, 189.111, 250.53]

	$provide vector<Shape<CylinderData> > nonIdentifiedCylinders;

	/*$nodeclass MoveCylinder : GrasperNode(GrasperRequest::moveTo) : doStart {
		NEW_SHAPEVEC(cyls, CylinderData, select_type<CylinderData>(worldShS));



		cout << "April tag's ID is " << april_tags[0]->getTagID() << endl;

		Point targetPoint;
		if(april_tags[0]->getTagID() == 10)
			targetPoint = Point(0,-1250,0,allocentric); // west goal box		
		else if(april_tags[0]->getTagID() == 11)
			targetPoint = Point(0,1250,0,allocentric); // east goal box		
		else {
			cout << "Didn't find expected ID in can's tag";
			postStateFailure();
		}

		NEW_SHAPE(destination, PointData, new PointData(worldShS, targetPoint));

		destination->setObstacle(false);
		graspreq.targetLocation = destination;
	}*/
/*
	$nodeclass LookAtTag : HeadPointerNode : doStart {
		getMC()->lookAtPoint(cyls[0]->getCentroid());
	}
*/
//=C=> PostureNode("lookatcan.pos")

	$nodeclass ReachUnidentifiedCylinder : GrasperNode(GrasperRequest::reach) : doStart {
		cout << "ReachUnidentifiedCylinder" << endl;
		NEW_SHAPEVEC(cyls, CylinderData, select_type<CylinderData>(worldShS));

		bool targetAcquired = false;
		Point targetPoint;
	
		$reference Lab8_p3::cylinderTagMap;
		if (!cylinderTagMap.empty()) {
			for(std::map<int,int>::iterator it = cylinderTagMap.begin();it != cylinderTagMap.end(); ++it) {
				SHAPEVEC_ITERATE(cyls, CylinderData, myCylinder) {
					if(it->first == myCylinder->getId() && it->second == -1) { //only unidentified cylinders
						cout << "got an unidentified cylinder!" << endl;
						cout << "coordX = " << myCylinder->getCentroid().coordX() << ", coordY = " << myCylinder->getCentroid().coordY() << endl;
						targetPoint = myCylinder->getCentroid();
						targetAcquired = true;
						cylinderTagMap.erase (it);
						break;
					}
				} END_ITERATE;
				if(targetAcquired)
					break;
			}

			if(!targetAcquired) {
				cout << "!targetAcquired" << endl;
				cancelThisRequest();
				postStateFailure();
			} else {
				cout << "targetAcquired" << endl;
				NEW_SHAPE(destination, PointData, new PointData(worldShS, targetPoint));
				destination->setObstacle(false);
				NEW_SHAPEVEC(polygons, PolygonData, select_type<PolygonData>(worldShS));		

				SHAPEVEC_ITERATE(polygons, PolygonData, myPolygon) {
					cout << "myPolygon->setObstacle(false) ID = " << myPolygon->getId() << endl;
					myPolygon->setObstacle(false);
				} END_ITERATE;

				graspreq.targetLocation = destination;

				for(std::map<int,int >::const_iterator it = cylinderTagMap.begin();it != cylinderTagMap.end(); ++it)
				    cout << it->first << "'s closest April Tag ID is " << it->second << endl;
			}
		} else {
			//all cylinders were easy and have been moved, yay!
			cout << "cylinderTagMap.empty()" << endl;
			cancelThisRequest();
			postStateSuccess();
		}

		//Your task is to put the cylinders with id 10 in the west goal box, and the cylinders with id 11 in the
	}

/*				
				float xOffsetDefault = 200.0;
				float yOffsetDefault = 200.0;
				xOffsetDefault = abs(targetPoint.coordX()) < xOffsetDefault ? abs(targetPoint.coordX()) : xOffsetDefault;
				yOffsetDefault = abs(targetPoint.coordY()) < yOffsetDefault ? abs(targetPoint.coordY()) : yOffsetDefault;
				float xOffset = targetPoint.coordX() > 0 ? -xOffsetDefault : xOffsetDefault;
				float yOffset = targetPoint.coordY() > 0 ? -yOffsetDefault : yOffsetDefault;
				NEW_SHAPE(destination, PointData, new PointData(worldShS, Point(targetPoint.coordX() + xOffset,targetPoint.coordY() + yOffset,0,allocentric)));
				destination->setObstacle(false);
				pilotreq.targetShape = destination;
*/