/// <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();
}
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.");
}