void testGoals()
{
GalileoSDK sdk = new GalileoSDK();
if (sdk.Connect("71329A5B0F2D68364BB7B44F3F125531E4C7F5BC3BCE2694DFE39B505FF9C730A614FF2790C1",
true, 10000, null,
null) != GALILEO_RETURN_CODE.OK)
{
Console.WriteLine("Connect to server failed");
}
while (true)
{
sdk.StartNav();
GalileoStatus status = sdk.GetCurrentStatus();
while (status.visualStatus != 1 || status.navStatus != 1)
{
Console.WriteLine("Wait for navigation initialization");
Console.WriteLine("status.visualStatus: " + status.visualStatus);
Console.WriteLine("status.navStatus: " + status.navStatus);
status = sdk.GetCurrentStatus();
Thread.Sleep(1000);
}
// 设置目标点
sdk.SetGoal(0);
// 等待goal status
status = sdk.GetCurrentStatus();
while (status.targetStatus != 1)
{
Console.WriteLine("Wait for goal start");
status = sdk.GetCurrentStatus();
Thread.Sleep(1000);
}
Console.WriteLine("Goal started");
// 暂停目标
Thread.Sleep(2 * 1000);
sdk.PauseGoal();
status = sdk.GetCurrentStatus();
while (status.targetStatus != 2)
{
Console.WriteLine("Wait for goal pause");
status = sdk.GetCurrentStatus();
Thread.Sleep(1000);
}
Console.WriteLine("Goal paused");
// 继续目标
Thread.Sleep(2 * 1000);
sdk.ResumeGoal();
status = sdk.GetCurrentStatus();
while (status.targetStatus != 1)
{
Console.WriteLine("Wait for goal resume");
status = sdk.GetCurrentStatus();
Thread.Sleep(1000);
}
Console.WriteLine("Goal resumed");
// 取消目标
Thread.Sleep(2 * 1000);
sdk.CancelGoal();
status = sdk.GetCurrentStatus();
while (status.targetStatus != 0 || status.targetNumID != -1)
{
Console.WriteLine("Wait for goal cancel");
status = sdk.GetCurrentStatus();
Thread.Sleep(1000);
}
Console.WriteLine("Goal cancelled");
// 再次设置目标
Thread.Sleep(2 * 1000);
Console.WriteLine("Set goal again");
sdk.SetGoal(0);
// 完成目标
status = sdk.GetCurrentStatus();
while (status.targetStatus != 1)
{
Console.WriteLine("Wait for goal start");
status = sdk.GetCurrentStatus();
Thread.Sleep(1000);
}
Console.WriteLine("Goal started");
while (status.targetStatus != 0 || status.targetNumID != 0)
{
Console.WriteLine("Wait for goal complete");
status = sdk.GetCurrentStatus();
Thread.Sleep(1000);
}
// 移动到特定目标
// 获取当前坐标
status = sdk.GetCurrentStatus();
float pos0_x = status.currentPosX;
float pos0_y = status.currentPosY;
// 再次设置目标 ，移动至1号目标点
Thread.Sleep(2 * 1000);
Console.WriteLine("Set goal again");
sdk.SetGoal(1);
// 完成目标
status = sdk.GetCurrentStatus();
while (status.targetStatus != 1)
{
Console.WriteLine("Wait for goal start");
status = sdk.GetCurrentStatus();
Thread.Sleep(1000);
}
Console.WriteLine("Goal started");
while (status.targetStatus != 0 || status.targetNumID != 1)
{
Console.WriteLine("Wait for goal complete");
status = sdk.GetCurrentStatus();
Thread.Sleep(1000);
}
// 再次获取坐标
status = sdk.GetCurrentStatus();
float pos1_x = status.currentPosX;
float pos1_y = status.currentPosY;
// 然后移动至0号和1号之间
byte goalNum = 0;
Console.WriteLine("Start move to " + (pos0_x + pos1_x) / 2 + " " + (pos0_y + pos1_y) / 2);
sdk.MoveTo((pos0_x + pos1_x) / 2, (pos0_y + pos1_y) / 2, ref goalNum);
// 等待移动完成
status = sdk.GetCurrentStatus();
while (status.targetStatus != 1)
{
Console.WriteLine("Wait for goal start");
status = sdk.GetCurrentStatus();
Thread.Sleep(1000);
}
Console.WriteLine("Goal started");
while (status.targetStatus != 0 || status.targetNumID != goalNum)
{
Console.WriteLine("Wait for goal complete");
Console.WriteLine("status.targetStatus: " + status.targetStatus);
Console.WriteLine("status.targetNumID: " + status.targetNumID);
Console.WriteLine("goalNum: " + goalNum);
status = sdk.GetCurrentStatus();
Thread.Sleep(1000);
}
Console.WriteLine("Move to " + (pos0_x + pos1_x) / 2 + " " + (pos0_y + pos1_y) / 2 + " complete");
Thread.Sleep(10 * 1000);
Console.WriteLine("All complete");
}
}