void testStartNav()
        {
            GalileoSDK sdk = new GalileoSDK();

            if (sdk.Connect("71329A5B0F2D68364BB7B44F3F125531E4C7F5BC3BCE2694DFE39B505FF9C730A614FF2790C1",
                            true, 10000, null,
                            null) != GALILEO_RETURN_CODE.OK)
            {
                Console.WriteLine("Connect to server failed");
            }
            while (true)
            {
                GalileoStatus status = sdk.GetCurrentStatus();
                Console.WriteLine("status: " + status.navStatus);
                Console.WriteLine("start nav");
                sdk.StartNav();
                Thread.Sleep(1000 * 20);
                status = sdk.GetCurrentStatus();
                Console.WriteLine("status: " + status.navStatus);
                Console.WriteLine("stop nav");
                sdk.StopNav();
                Thread.Sleep(1000 * 20);
                status = sdk.GetCurrentStatus();
                Console.WriteLine("status : " + status.navStatus);
            }
        }
        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");
            }
        }