Exemplo n.º 1
0
        /// <summary>
        /// Service Start
        /// </summary>
        protected override void Start()
        {
            base.Start();
            // Add service specific initialization here.

            if (_state == null)
            {
                _state = new SoarMSRState();
            }

            // There should be some code in here to validate the settings
            // from the config file just in case the user entered some
            // invalid values ...

            _halfObstacleAngleRange = _state.ObstacleAngleRange / 2.0f;

            // Now save the State
            // This creates a new file the first time it is run.
            // Later, it re-reads the existing file, but by then
            // the file has been populated with the default values.
            SaveState(_state);

            _soar.Log         += LogHandler;
            _soar.DriveOutput += DriveOutputHandler;
            _soar.InitializeSoar(_state);

            SubscribeToBumpers();
            SubscribeToSickLRF();

            // Start Soar after everything is loaded
            Thread soarThread = new Thread(new ThreadStart(_soar.RunSoar));

            soarThread.Start();
        }
Exemplo n.º 2
0
        public void InitializeSoar(SoarMSRState initialState)
        {
            if (_kernel != null)
            {
                throw new Exception("Soar: Already initialized");
            }

            _kernel = sml.Kernel.CreateKernelInNewThread("SoarKernelSML");
            if (_kernel.HadError())
            {
                _kernel = null;
                throw new Exception("Soar: Error initializing kernel: " + _kernel.GetLastErrorDescription());
            }

            _running = false;
            _stop    = false;

            _agent = _kernel.CreateAgent(initialState.AgentName);

            // We test the kernel for an error after creating an agent as the agent
            // object may not be properly constructed if the create call failed so
            // we store errors in the kernel in this case.  Once this create is done we can work directly with the agent.
            if (_kernel.HadError())
            {
                throw new Exception("Soar: Error creating agent: " + _kernel.GetLastErrorDescription());
            }

            _kernel.SetAutoCommit(false);
            _agent.SetBlinkIfNoChange(false);

            bool result = _agent.LoadProductions(initialState.Productions);

            if (!result)
            {
                throw new Exception("Soar: Error loading productions " + initialState.Productions
                                    + " (current working directory: " + _agent.ExecuteCommandLine("pwd") + ")");
            }

            // reset state
            Bumper   = new BumperState();
            Override = new OverrideState();
            Obstacle = false;

            // Prepare communication channel
            Identifier inputLink = _agent.GetInputLink();

            if (inputLink == null)
            {
                throw new Exception("Soar: Error getting the input link");
            }

            Identifier overrideWME = _agent.CreateIdWME(inputLink, "override");

            _overrideActiveWME = _agent.CreateStringWME(overrideWME, "active", "false");
            Identifier drivePowerWME = _agent.CreateIdWME(overrideWME, "drive-power");

            _overrideLeftWME  = _agent.CreateFloatWME(drivePowerWME, "left", 0);
            _overrideRightWME = _agent.CreateFloatWME(drivePowerWME, "right", 0);
            _overrideStopWME  = _agent.CreateStringWME(drivePowerWME, "stop", "false");

            Identifier configWME = _agent.CreateIdWME(inputLink, "config");

            Identifier powerWME = _agent.CreateIdWME(configWME, "power");

            _agent.CreateFloatWME(powerWME, "drive", initialState.DrivePower);
            _agent.CreateFloatWME(powerWME, "reverse", initialState.ReversePower);

            Identifier delayWME = _agent.CreateIdWME(configWME, "delay");

            _agent.CreateFloatWME(delayWME, "stop", initialState.StopTimeout);
            _agent.CreateFloatWME(delayWME, "reverse", initialState.BackUpTimeout);
            _agent.CreateFloatWME(delayWME, "turn", initialState.TurnTimeout);
            _agent.CreateFloatWME(delayWME, "variance", initialState.TimeoutVariance);

            Identifier sensorsWME = _agent.CreateIdWME(inputLink, "sensors");

            Identifier bumperWME = _agent.CreateIdWME(sensorsWME, "bumper");
            Identifier frontWME  = _agent.CreateIdWME(bumperWME, "front");

            _frontBumperPressedWME    = _agent.CreateStringWME(frontWME, "pressed", "false");
            _frontBumperWasPressedWME = _agent.CreateStringWME(frontWME, "was-pressed", "false");
            Identifier rearWME = _agent.CreateIdWME(bumperWME, "rear");

            _rearBumperPressedWME    = _agent.CreateStringWME(rearWME, "pressed", "false");
            _rearBumperWasPressedWME = _agent.CreateStringWME(rearWME, "was-pressed", "false");

            Identifier sickLRFWME = _agent.CreateIdWME(sensorsWME, "sicklrf");

            _obstacleWME = _agent.CreateStringWME(sickLRFWME, "obstacle", "false");

            // Current time WME
            _timeWME = _agent.CreateFloatWME(inputLink, "time", 0);

            // Random number WME and supporting state
            _randomWME = _agent.CreateFloatWME(inputLink, "random", 0);
            if (initialState.HasRandomSeed)
            {
                _random = new Random(initialState.RandomSeed);
                Trace.WriteLine("Seeding Soar's random number generator.");
                _agent.ExecuteCommandLine("srand " + initialState.RandomSeed);
                if (_agent.HadError())
                {
                    throw new Exception("Failed to seed Soar's random number generator");
                }
            }
            else
            {
                _random = new Random();
            }

            // commit input link structure
            _agent.Commit();

            _updateCall = new sml.Kernel.UpdateEventCallback(UpdateEventCallback);
            _kernel.RegisterForUpdateEvent(sml.smlUpdateEventId.smlEVENT_AFTER_ALL_OUTPUT_PHASES, _updateCall, null);

            // spawn debugger
            if (initialState.SpawnDebugger)
            {
                SpawnDebugger();
            }

            _simulationStart = DateTime.Now;

            OnLog("Soar initialized.");
        }