public IEnumerator TrajectoryPublisher_PickAndPlaceDemo_CompletesTask()
{
#if INTEGRATION_TEST
SetUpScene();
// TODO: This test could be made a PlayMode test once ImportRobot can use the PlayMode URDF import
ImportRobot();
CreateRosConnection();
CreateTrajectoryPlannerPublisher();
yield return(new EnterPlayMode());
m_TargetPlacement = GameObject.Find(k_NameTargetPlacement).GetComponent <TargetPlacement>();
Assert.IsNotNull(m_TargetPlacement, $"Unable to find {nameof(TargetPlacement)} attached to a " +
$"GameObject called {k_NameTargetPlacement} in scene.");
var publisher = GameObject.Find(k_NamePublisher).GetComponent <TrajectoryPlanner>();
publisher.PublishJoints();
while (!DidPlacementSucceed && m_TimeElapsedSeconds < k_TestTimeoutSeconds)
{
m_TimeElapsedSeconds += Time.deltaTime;
yield return(null);
}
Assert.IsTrue(DidPlacementSucceed, "Pick and Place did not complete before test timed out.");
yield return(new ExitPlayMode());
}
static IEnumerator WaitForState(
TargetPlacement targetPlacement, TargetPlacement.PlacementState stateExpected, int numFramesToWait)
{
var numFramesTested = 0;
while (targetPlacement.CurrentState != stateExpected && numFramesTested < numFramesToWait)
{
numFramesTested++;
yield return(null);
}
Assert.AreEqual(stateExpected, targetPlacement.CurrentState);
}
public void TestPoi()
{
TargetPlacementObject poi = new TargetPlacementObject
{
id = 0,
isValid = true,
name = "testPoi",
position = Vector3.one
};
var fbb = new FlatBufferBuilder(1024);
var offset = poi.ToBuffer(fbb);
fbb.Finish(offset.Value);
var bArray = fbb.SizedByteArray();
var bb = new ByteBuffer(bArray);
var desPoi = TargetPlacement.GetRootAsTargetPlacement(bb);
print("Poi Test : " + (ComparePOI(poi, desPoi) ? "Passed" : "Failed"));
}
private bool ComparePOI(TargetPlacementObject poi, TargetPlacement desPoi)
{
return(desPoi.Id == poi.id && desPoi.Name == poi.name &&
new Vector3(desPoi.Position.Value.X, desPoi.Position.Value.Y, desPoi.Position.Value.Z) ==
poi.position);
}
