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;


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

        DifferentialDriveEntity _robot = null;

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

		[ServiceState]
		SimulationFromState4State _state = new SimulationFromState4State();
		
		[ServicePort("/SimulationFromState4", AllowMultipleInstances = true)]
		SimulationFromState4Operations _mainPort = new SimulationFromState4Operations();
		
		public SimulationFromState4Service(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);
           
            //Start driving
            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)
                                    {                                        
                                        LogInfo(LogGroups.Console, "<< Bumper event >> " + update.Body.Pressed);
                                    }
                                )),
                            new ConcurrentReceiverGroup()
                            )
                    );

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

            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);

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

            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 SimulationFromState4
{
	public sealed class Contract
	{
		[DataMember]
		public const string Identifier = "http://schemas.tempuri.org/2009/12/simulationfromstate4.html";
	}
	
	[DataContract]
	public class SimulationFromState4State
	{
	}
	
	[ServicePort]
	public class SimulationFromState4Operations : PortSet
	{
	}
	
	public class Get : Get>
	{
		public Get()
		{
		}
		
		public Get(GetRequestType body)
			: base(body)
		{
		}
		
		public Get(GetRequestType body, PortSet responsePort)
			: base(body, responsePort)
		{
		}
	}
}