StartSimulationEngine  "SimState/basicsim.xml"


AddNewEntity    wall1  	/Position:-0.5  0.25  -2.5
AddBoxShape  	/Dimensions:3.0  0.5  0.5  	/Mass:1.0	/Texture:"WoodFloor.jpg"

AddNewEntity    wall2  	/Position:-0.5  0.25  2.5
AddBoxShape  	/Dimensions:3.0  0.5  0.5  	/Mass:1.0	/Texture:"WoodFloor.jpg"

AddNewEntity    wall3  	/Position:-2.5  0.25  0
AddBoxShape  	/Dimensions:0.5  0.5  4.5  	/Mass:1.0	/Texture:"WoodFloor.jpg"

AddNewEntity    wall4  	/Position:2.5  0.25  0
AddBoxShape  	/Dimensions:0.5  0.5  4.5  	/Mass:1.0	/Texture:"WoodFloor.jpg"



AddDifferentialDriveEntity    myrobot

AddBumperEntity    bumper1	/ParentEntity:myrobot
	/FrontPosition:0    0.1    -0.3
	/RearPosition:0    0.1    0.3

AddLaserRangeFinderEntity    lrf1	/Position:0  0.4  0	/ParentEntity:myrobot

FlushScript  

wait 10000
SaveSceneAs    "SimState/boxsim.xml"  

dssprojectmigration "here_your_unzipped_folder_name"	

using System;
using System.Collections.Generic;
using System.ComponentModel;
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using W3C.Soap;
using System.IO;

using dssp = Microsoft.Dss.ServiceModel.Dssp;
using engineproxy = Microsoft.Robotics.Simulation.Engine.Proxy;
using submgr = Microsoft.Dss.Services.SubscriptionManager;
using simengine = Microsoft.Robotics.Simulation.Engine;
using physics = Microsoft.Robotics.Simulation.Physics;

using Microsoft.Robotics.Simulation;
using Microsoft.Robotics.Simulation.Engine;
using Microsoft.Robotics.Simulation.Physics;
using Microsoft.Robotics.PhysicalModel;

using contactsensor = Microsoft.Robotics.Services.ContactSensor.Proxy;
using sicklrf = Microsoft.Robotics.Services.Sensors.SickLRF.Proxy;


namespace SimulationFromState7
{
	[Contract(Contract.Identifier)]
	[DisplayName("SimulationFromState7")]
	[Description("SimulationFromState7 service (no description provided)")]
	class SimulationFromState7Service : DsspServiceBase
	{
        simengine.SimulationEnginePort _notificationTarget;

        DifferentialDriveEntity _robot = null;

        contactsensor.ContactSensorArrayOperations _bumperPort = new contactsensor.ContactSensorArrayOperations();
        contactsensor.ContactSensorArrayOperations _bumperNotifyPort = new contactsensor.ContactSensorArrayOperations();

        sicklrf.SickLRFOperations _lrfPort = new sicklrf.SickLRFOperations();
        sicklrf.SickLRFOperations _lrfNotifyPort = new sicklrf.SickLRFOperations();

        string _currentMode = "Ready";

		[ServiceState]
		SimulationFromState7State _state = new SimulationFromState7State();
		
		[ServicePort("/SimulationFromState7", AllowMultipleInstances = true)]
		SimulationFromState7Operations _mainPort = new SimulationFromState7Operations();
		
		public SimulationFromState7Service(DsspServiceCreationPort creationPort)
			: base(creationPort)
		{
		}

        protected override void Start()
        {
            _notificationTarget = new simengine.SimulationEnginePort();

            base.Start();


            MainPortInterleave.CombineWith(
                new Interleave(
                    new TeardownReceiverGroup(),
                    new ExclusiveReceiverGroup(
                        Arbiter.Receive(true, _notificationTarget, InsertEntityNotificationHandler),
                        Arbiter.Receive(true, _notificationTarget, DeleteEntityNotificationHandler)
                    ),
                    new ConcurrentReceiverGroup()
                )
            );


            //Start simulation engine from the state file
            string filename = "SimState/boxsim.xml";

            dssp.PartnerType pt = new dssp.PartnerType();
            System.Xml.XmlQualifiedName qName = 
            	new System.Xml.XmlQualifiedName("StateService", "http://schemas.microsoft.com/xw/2004/10/dssp.html");
            pt.Name = qName;
            pt.Service = filename;
            pt.Contract = "http://schemas.microsoft.com/robotics/2006/04/simulationengine.html";

            engineproxy.Contract.CreateService(ConstructorPort, new PartnerType[] { pt });


            //Request to robot platform bilding
            simengine.EntitySubscribeRequestType esrt = new simengine.EntitySubscribeRequestType();
            esrt.Name = "myrobot";
            simengine.SimulationEngine.GlobalInstancePort.Subscribe(esrt, _notificationTarget);
        }


        void InsertEntityNotificationHandler(simengine.InsertSimulationEntity ins)
        {
            if (ins.Body.State.Name == "myrobot")
            {
                _robot = (DifferentialDriveEntity)ins.Body;

                LogInfo(LogGroups.Console, _robot.State.Name + " was connected!");

                SpawnIterator(ConnectSensors);
            }
        }


        void DeleteEntityNotificationHandler(simengine.DeleteSimulationEntity del)
        {
            if (del.Body.State.Name == "myrobot")
            {
                _robot = null;
            }
        }


        public IEnumerator ConnectSensors()
        {
            //Connect BumperSensor
            SpawnIterator(ConnectBumperSensor);

            //Connect LRFSensor
            SpawnIterator(ConnectLRFSensor);

            //Start driving
            _currentMode = "Ready";
            SpawnIterator(DriveRobot);

            yield break;
        }
        

        public IEnumerator ConnectBumperSensor()
        {
            string targetname = "bumper1";
            string contract = "http://schemas.helloapps.com/2009/09/splextsimulatedbumper.html";

            ServiceInfoType sInfo = new ServiceInfoType(contract);

            dssp.PartnerType pt = null;
            pt = new dssp.PartnerType();
            System.Xml.XmlQualifiedName qName =
                new System.Xml.XmlQualifiedName("Entity", "http://schemas.microsoft.com/robotics/2006/04/simulation.html");
            pt.Name = qName;
            pt.Service = "http://localhost/" + targetname;

            sInfo.PartnerList.Add(pt);


            PortSet createResultPort;

            createResultPort = base.CreateService(sInfo);

            yield return createResultPort.Choice();

            CreateResponse success = createResultPort;

            if (success != null)
            {
                LogInfo(LogGroups.Console, "(( Bumper Binding Success -> " + targetname + " ))");

                _bumperPort = ServiceForwarder(success.Service);

                MainPortInterleave.CombineWith(
                            new Interleave(
                            new TeardownReceiverGroup(),
                            new ExclusiveReceiverGroup(
                                Arbiter.Receive(true, _bumperNotifyPort,
                                    delegate(contactsensor.Update update)
                                    {                                                                               
                                        if (update.Body.Pressed)
                                        {
                                            if (_currentMode == "Ready" || _currentMode == "Going")
                                            {
                                                LogInfo(LogGroups.Console, "<< Bumper touched >>");

                                                _currentMode = "BumperTouched";
                                                SpawnIterator(DriveRobot);
                                            }
                                        }
                                    }
                                )),
                            new ConcurrentReceiverGroup()
                            )
                    );

                _bumperPort.Subscribe(_bumperNotifyPort, typeof(contactsensor.Update));
            }

            yield break;
        }


        public IEnumerator ConnectLRFSensor()
        {
            string targetname = "lrf1";
            string contract = "http://schemas.microsoft.com/robotics/simulation/services/2006/05/simulatedlrf.html";

            ServiceInfoType sInfo = new ServiceInfoType(contract);

            dssp.PartnerType pt = null;
            pt = new dssp.PartnerType();
            System.Xml.XmlQualifiedName qName =
                new System.Xml.XmlQualifiedName("Entity", "http://schemas.microsoft.com/robotics/2006/04/simulation.html");
            pt.Name = qName;
            pt.Service = "http://localhost/" + targetname;

            sInfo.PartnerList.Add(pt);


            PortSet createResultPort;

            createResultPort = base.CreateService(sInfo);

            yield return createResultPort.Choice();

            CreateResponse success = createResultPort;

            if (success != null)
            {
                LogInfo(LogGroups.Console, "(( LRF Binding Success -> " + targetname + " ))");

                _lrfPort = ServiceForwarder(success.Service);

                MainPortInterleave.CombineWith(
                            new Interleave(
                            new TeardownReceiverGroup(),
                            new ExclusiveReceiverGroup(
                                Arbiter.Receive(true, _lrfNotifyPort,
                                    delegate(sicklrf.Replace replace)
                                    {
                                        if (replace.Body.DistanceMeasurements != null)
                                        {
                                            if (replace.Body.DistanceMeasurements[180] < 1000)
                                            {
                                                if (_currentMode == "Ready" || _currentMode == "Going")
                                                {
                                                    LogInfo(LogGroups.Console, "<< LRF detected >>");

                                                    _currentMode = "LRFDetected";
                                                    SpawnIterator(DriveRobot);
                                                }
                                            }
                                        }
                                    }
                                )),
                            new ConcurrentReceiverGroup()
                            )
                    );

                _lrfPort.Subscribe(_lrfNotifyPort, typeof(sicklrf.Replace));
            }

            yield break;
        }


        public IEnumerator DriveRobot()
        {
            if (_robot != null)
            {
                Port entityResponse = new Port();

                DriveAction da = new DriveAction();
                da.DiffDrive = _robot;
                da.EntityResponse = entityResponse;

                Task deferredTask = new Task(da, ExecuteDriveDistance);


                switch (_currentMode)
                {
                    case "Ready":
                        {
                            _currentMode = "Going";

                            //Go forwards with 0.2 and 0.2 powers                        
                            da.LeftPower = 0.2f;
                            da.RightPower = 0.2f;

                            deferredTask = new Task(da, ExecuteSetDrivePower);
                            da.DiffDrive.DeferredTaskQueue.Post(deferredTask);

                            break;
                        }

                    case "BumperTouched":
                        {
                            //Go back
                            da.Distance = 0.5f;
                            da.MotorPower = -0.2f;

                            deferredTask = new Task(da, ExecuteDriveDistance);
                            da.DiffDrive.DeferredTaskQueue.Post(deferredTask);

                            yield return Arbiter.Receive(false, entityResponse,
                                delegate(simengine.OperationResult result)
                                {
                                }
                            );

                            //Turn 90 degrees with 0.2 power
                            da.Degrees = 90.0f;
                            da.MotorPower = 0.2f;

                            deferredTask = new Task(da, ExecuteRotateDegrees);
                            da.DiffDrive.DeferredTaskQueue.Post(deferredTask);

                            yield return Arbiter.Receive(false, entityResponse,
                                delegate(simengine.OperationResult result)
                                {
                                }
                            );

                            _currentMode = "Ready";

                            SpawnIterator(DriveRobot);

                            break;
                        }

                    case "LRFDetected":
                        {
                            //Go back
                            da.Distance = 0.5f;
                            da.MotorPower = -0.2f;

                            deferredTask = new Task(da, ExecuteDriveDistance);
                            da.DiffDrive.DeferredTaskQueue.Post(deferredTask);

                            yield return Arbiter.Receive(false, entityResponse,
                                delegate(simengine.OperationResult result)
                                {
                                }
                            );

                            //Turn 90 degrees with 0.2 power
                            da.Degrees = 90.0f;
                            da.MotorPower = 0.2f;

                            deferredTask = new Task(da, ExecuteRotateDegrees);
                            da.DiffDrive.DeferredTaskQueue.Post(deferredTask);

                            yield return Arbiter.Receive(false, entityResponse,
                                delegate(simengine.OperationResult result)
                                {
                                }
                            );

                            _currentMode = "Ready";

                            SpawnIterator(DriveRobot);

                            break;
                        }                    
                }
            }

            yield break;
        }

        void ExecuteSetDrivePower(DriveAction da)
        {
            if (da.DiffDrive.IsEnabled)
                da.DiffDrive.IsEnabled = true;

            da.DiffDrive.SetMotorTorque(da.LeftPower, da.RightPower);
        }


        void ExecuteRotateDegrees(DriveAction da)
        {
            if (da.DiffDrive.IsEnabled)
                da.DiffDrive.IsEnabled = true;

            da.DiffDrive.RotateDegrees(da.Degrees, da.MotorPower, da.EntityResponse);
        }

        void ExecuteDriveDistance(DriveAction da)
        {
            if (da.DiffDrive.IsEnabled)
                da.DiffDrive.IsEnabled = true;

            da.DiffDrive.DriveDistance(da.Distance, da.MotorPower, da.EntityResponse);
        }
	}


    public class DriveAction
    {
        public DifferentialDriveEntity DiffDrive = null;

        public float MotorPower = 0.0f;

        public float LeftPower = 0.0f;

        public float RightPower = 0.0f;

        public float Degrees = 0.0f;

        public float Distance = 0.0f;

        public Port EntityResponse = null;
    }
}

using System;
using System.Collections.Generic;
using System.ComponentModel;
using Microsoft.Ccr.Core;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using W3C.Soap;

namespace SimulationFromState7
{
	public sealed class Contract
	{
		[DataMember]
		public const string Identifier = "http://schemas.tempuri.org/2009/12/simulationfromstate7.html";
	}
	
	[DataContract]
	public class SimulationFromState7State
	{
	}
	
	[ServicePort]
	public class SimulationFromState7Operations : PortSet
	{
	}
	
	public class Get : Get>
	{
		public Get()
		{
		}
		
		public Get(GetRequestType body)
			: base(body)
		{
		}
		
		public Get(GetRequestType body, PortSet responsePort)
			: base(body, responsePort)
		{
		}
	}
}