Добавил:
Upload Опубликованный материал нарушает ваши авторские права? Сообщите нам.
Вуз: Предмет: Файл:

Задани на лабораторные работы. ПРК / Professional Microsoft Robotics Developer Studio

.pdf
Скачиваний:
126
Добавлен:
20.04.2015
Размер:
16.82 Mб
Скачать

www.it-ebooks.info

Chapter 6: Extending the MRDS Visual Simulation Environment

false, _mainPort,

DefaultDropHandler)

),u

new ExclusiveReceiverGroup(), new ConcurrentReceiverGroup()

));

// start notification method SpawnIterator<DateTime>(DateTime.Now, CheckForStateChange);

}

8. The SpawnIterator call at the end of the Start method is used to start up a method that periodically checks for a change in the reading from the IR sensor and sends a notification to subscribers if necessary. Add this code for that method. When this method runs, it checks whether the Distance property on the entity has changed. If it has, the service updates its state from the entity and then sends a notification to all subscribed services. The method then sets itself to wake up again after 200 ms have elapsed.

float _previousDistance = 0;

private IEnumerator<ITask> CheckForStateChange(DateTime timeout)

{

while (true)

{

if (_entity != null)

{

if (_entity.Distance != _previousDistance)

{

// send notification of state change UpdateState();

base.SendNotification<Replace>(_submgrPort, _state);

}

_previousDistance = _entity.Distance;

}

yield return Arbiter.Receive(false, TimeoutPort(200), delegate { });

}

}

9. Add the following code to create a default state for the service and to update the service state from the CorobotIR entity. The state is defined as part of the AnalogSensor contract. It includes a raw measurement, which is equal to the Distance property on the entity; a RawMeasurementRange, which reflects the maximum value the sensor can have; and a NormalizedMeasurement value, which is the raw value normalized against the maximum value:

private void CreateDefaultState()

{

_state.HardwareIdentifier = 0; _state.NormalizedMeasurement = 0;

_state.Pose = new Microsoft.Robotics.PhysicalModel.Proxy.Pose(); _state.RawMeasurement = 0;

_state.RawMeasurementRange = _entity.MaximumRange;

}

(continued)

333

www.it-ebooks.info

Part II: Simulations

(continued)

void UpdateState()

{

// update our state from the entity _state.RawMeasurement = _entity.Distance; _state.NormalizedMeasurement =

_state.RawMeasurement / _state.RawMeasurementRange; _state.TimeStamp = DateTime.Now;

}

10. Add the following code to receive notifications from the SimulationEngine service. These methods are nearly identical to the corresponding methods in the SimulatedQuadDifferentialDrive service:

void InsertEntityNotificationHandlerFirstTime(simengine.InsertSimulationEntity ins)

{

InsertEntityNotificationHandler(ins);

base.Start();

// Add service specific initialization here. MainPortInterleave.CombineWith(

new Interleave(

new TeardownReceiverGroup(), new ExclusiveReceiverGroup(

Arbiter.Receive<simengine.InsertSimulationEntity>( true,

_notificationTarget, InsertEntityNotificationHandler),

Arbiter.Receive<simengine.DeleteSimulationEntity>( true,

_notificationTarget, DeleteEntityNotificationHandler)

),

new ConcurrentReceiverGroup()

)

);

}

void InsertEntityNotificationHandler(simengine.InsertSimulationEntity ins)

{

_entity = (corobot.CorobotIREntity)ins.Body; _entity.ServiceContract = Contract.Identifier;

CreateDefaultState();

}

void DeleteEntityNotificationHandler(simengine.DeleteSimulationEntity del)

{

_entity = null;

}

334

www.it-ebooks.info

Chapter 6: Extending the MRDS Visual Simulation Environment

11. Add a call to UpdateState before the state is posted to the response port in the GetHandler method:

[ServiceHandler(ServiceHandlerBehavior.Concurrent)]

public virtual IEnumerator<ITask> GetHandler(pxanalogsensor.Get get)

{

UpdateState(); get.ResponsePort.Post(_state); yield break;

}

12. Add a call to SubscribeHelper in the SubscribeHandler to manage subscription requests. Handling subscriptions is described in Service Tutorial 4 in the SDK documentation.

public virtual IEnumerator<ITask> SubscribeHandler(pxanalogsensor.Subscribe subscribe)

{

SubscribeHelper( _submgrPort, subscribe.Body, subscribe.ResponsePort);

yield break;

}

13. Add the following entries to the Corobot.manifest.xml file to start two copies of this new service. In the Corobot service, you gave the name “Corobot_frontIR” to the front IR sensor and “Corobot_rearIR” to the rear sensor. The entity partners associated with each of these services have those same names. You also used the <dssp:Service> attribute to specify a name for the service so that you can distinguish the front IR service from the rear one.

<!-- Start Front IR service --> <ServiceRecordType>

<dssp:Contract>http://schemas.tempuri.org/2007/08/simulatedir.html</dssp:Contract> <dssp:Service>http://localhost/Corobot/FrontIR</dssp:Service>

<dssp:PartnerList> <dssp:Partner>

<!--The partner name must match the entity name--> <dssp:Service>http://localhost/Corobot_frontIR</dssp:Service> <dssp:Name>simcommon:Entity</dssp:Name>

</dssp:Partner> </dssp:PartnerList>

</ServiceRecordType>

<!-- Start Rear IR service --> <ServiceRecordType>

<dssp:Contract>http://schemas.tempuri.org/2007/08/simulatedir.html</dssp:Contract> <dssp:Service>http://localhost/Corobot/RearIR</dssp:Service>

<dssp:PartnerList> <dssp:Partner>

<!--The partner name must match the entity name-->

(continued)

335

www.it-ebooks.info

Part II: Simulations

(continued)

<dssp:Service>http://localhost/Corobot_rearIR</dssp:Service> <dssp:Name>simcommon:Entity</dssp:Name>

</dssp:Partner> </dssp:PartnerList>

</ServiceRecordType>

That’s it. You should now have a working SimulatedIR service. Verify it by running the Corobot manifest. Start a browser window and navigate to http://localhost:50000 and select Service Directory from the left column. You should see something similar to what is shown in Figure 6-13. Each service is listed twice because DssHost adds two entries to the service directory for services that support an alternate contract: one for their own contract and one for the alternate contract they support. In this case, both entries refer to the same port. Click each of the SimulatedIR services to see their current state. As you drive the Corobot around in the environment, refresh the service state to verify that the RawMeasurement field correctly reflects the distance from that IR sensor to the giant box.

Figure 6-13

Summary

That completes the basic functionality for the Corobot entity and its associated services. This entity can now be used in a variety of simulation scenarios, such as the Robo-Magellan scenario covered in the next chapter.

This chapter began with an overview of the methods and types provided by the various simulation DLLs, including the characteristics of the VisualEntity type, which is fundamental to creating new simulation entities. The characteristics of a simulation service were also described, including their relationship to orchestration services such as the SimMagellan service.

You created a new simulation service called Corobot that added entities to the simulation environment. This eventually included a model of the Corobot robot. You then defined a

SimulatedQuadDifferentialDrive service and used it to drive the Corobot around the environment using the SimpleDashboard service. You tuned the top speed of the entity and the tire friction and then you made the wheels turn as the robot moves. Finally, you added a detailed 3D mesh to the Corobot model to make it look more realistic, and then you added a camera and defined a Simulated IR Distance sensor entity and service so that you could add IR sensors to the front and rear of the Corobot.

The entities and services defined in this chapter provide examples for you to use as you create your own custom entities and their associated services.

336

www.it-ebooks.info

Using Orchestration

Services to Build a

Simulation Scenario

A simulation scenario is an environment and one or more robot models that is used to prototype a control algorithm. The extent to which the environment reflects real-world conditions and limitations depends on the desired results from the simulation. If it is important that the exact code you use in the simulator can run on a real robot, then the simulated world and services must carefully model every important aspect of the real world. If the simulator is to be used only to prototype or demonstrate an algorithm, then real-world fidelity is less important.

In this chapter, you’ll use the new Corobot robot entity from the last chapter in a simulation scenario that mimics the Seattle Robotics Society’s Robo-Magellan competition. A description of the competition and the rules can be found at www.robothon.org/robothon/robo-magellan

.php. Before the competition begins, a referee places cones on the course and then provides GPS coordinates to the contestants for their location. Robots start at a predetermined place and navigate to each cone, avoiding obstacles along the way. The challenges that the robots face are navigating over outdoor terrain, avoiding obstacles, and dealing with GPS signal problems.

In this simulation, you’ll mainly focus on navigation and obstacle avoidance. You haven’t built a

GPS service yet so you’ll use the robot position reported by the SimulatedQuadDifferentialDrive to mimic this capability.

In the last chapter, you used the Corobot manifest to test the Corobot entity and its associated services. For this scenario, you’ll define a new manifest that runs the Corobot services along with a referee service and the Robo-Magellan orchestration service. Figure 7-1 shows the completed Robo-Magellan scenario.

www.it-ebooks.info

Part II: Simulations

Figure 7-1

The Robo-Magellan Referee Service

The first thing you’ll need is a referee to set up the course and place the cones. In a scenario like this one, it is a good idea to separate the player functionality from the referee functionality. That way, the interface between the two is well defined and it is eventually easier to move the player service to an actual robot.

The referee service will be responsible for setting up all of the entities in the simulation environment, including the sky, sun, and ground entities as well as the obstacles in the world.

Instead of using DssNewService to create the referee service, you’re going to copy Simulation Tutorial 1 from the SDK and modify it. Simulation Tutorial 1 is already set up to interact with the simulation environment and create environment entities, so it is already very close to what you want your referee to be. You can create a referee service using the following commands from the MRDS command prompt. The text that you type is shown in bold.

C:\Microsoft Robotics Studio (1.5)\ProMRDS>mkdir MyChapter7

C:\Microsoft Robotics Studio (1.5)\ProMRDS>cd MyChapter7

C:\Microsoft Robotics Studio (1.5)\ProMRDS\MyChapter7>mkdir Referee

C:\Microsoft Robotics Studio (1.5)\ProMRDS\MyChapter7>

copy ..\..\samples\simulationtutorials\tutorial1\*.* Referee

..\..\samples\simulationtutorials\tutorial1\AssemblyInfo.cs

..\..\samples\simulationtutorials\tutorial1\SimulationTutorial1.cs

..\..\samples\simulationtutorials\tutorial1\SimulationTutorial1.csproj

..\..\samples\simulationtutorials\tutorial1\SimulationTutorial1.csproj.user

..\..\samples\simulationtutorials\tutorial1\SimulationTutorial1.sln 5 file(s) copied.

338

www.it-ebooks.info

Chapter 7: Using Orchestration Services to Build a Simulation Scenario

Customizing the SimulationTutorial1 Service

Now you’ll rename the files and change the namespace and contract identifier to make this service your own. Start by renaming the solution file and opening it from the command line as follows. Again, text that you type is shown in bold:

C:\Microsoft Robotics Studio (1.5)\ProMRDS\MyChapter7\Referee>ren

SimulationTutorial1.sln Referee.sln

C:\Microsoft Robotics Studio (1.5)\ProMRDS\MyChapter7\Referee> Referee.sln

Use the following steps to make the Referee project:

1.

2.

3.

4.

5.

6.

7.

Rename the SimulationTutorial1 project to Referee and rename SimulationTutorial1.cs to Referee.cs inside Visual Studio.

Open Referee.cs. Change the namespace from Robotics.SimulationTutorial1 to

ProMRDS.Simulation.MagellanReferee.

Rename the SimulationTutorial1 class to MagellanReferee and update the DisplayName and Description attributes.

Change the contract identifier at the bottom of the file to http://schemas.tempuri.org/ 2007/08/MagellanReferee.htm.

Do a global replace of SimulationTutorial1 with MagellanReferee.

Open the solution properties and change the assembly name to SimMagellanReferee.Y2007

.M08, and change the default namespace to ProMRDS.Simulation.MagellanReferee.

The solution should now compile with no errors.

Starting a Service from the Browser

You don’t have a manifest to run this service yet, so you’ll use a different method to run it. Start a Dss Node at port 50000 without specifying a manifest:

C:\Microsoft Robotics Studio (1.5)\ProMRDS\MyChapter7\Referee>cd ..\..\..

C:\Microsoft Robotics Studio (1.5)>dsshost -p:50000 -t:50001

*Service uri: [09/10/2007 08:12:33][http://msrs1:50000/directory]

*Service uri: [09/10/2007 08:12:33][http://msrs1:50000/constructor/ef2ee88f- cf29-4052-9168-a6714fb53ae1]

*No initial manifest supplied. [09/10/2007 08:12:34][http://msrs1:50000/ manifestloaderclient]

Open a browser window and navigate to http://localhost:50000. Select Control Panel from the left column and type referee in the Search box. You should see the new Magellan Referee service displayed with the DisplayName and Description you specified in the service. Click the Create button on the right and you should soon see the simulation window appear with the scene from Simulation Tutorial 1.

339

www.it-ebooks.info

Part II: Simulations

Building the World and Adding Cameras

Now you’ll modify the referee service to build a world for our scenario. Replace the contents of the PopulateWorld method with the following:

// Set up initial view

CameraView view = new CameraView(); view.EyePosition = new Vector3(-0.91f, 0.67f, -1f); view.LookAtPoint = new Vector3(1.02f, 0.09f, 0.19f); SimulationEngine.GlobalInstancePort.Update(view);

This sets the initial camera viewpoint to something convenient:

// Add another camera to view the scene from above CameraEntity fromAbove = new CameraEntity(640, 480); fromAbove.State.Name = “FromAbove”;

fromAbove.Location = new xna.Vector3(4.3f, 20.59f, 0.86f); fromAbove.LookAt = new xna.Vector3(4.29f, 18.26f, 0.68f); fromAbove.IsRealTimeCamera = false; SimulationEngine.GlobalInstancePort.Insert(fromAbove);

You also need to add a using statement, as shown in the following snippet, and a reference to

Microsoft.Xna.Framework.DLL, as described in the previous chapter:

using xna = Microsoft.Xna.Framework;

Now add a second camera that looks at the whole scene from above. It is quick to switch to this camera using F8:

// Add a SkyDome.

SkyDomeEntity sky = new SkyDomeEntity(“skydome.dds”, “sky_diff.dds”); SimulationEngine.GlobalInstancePort.Insert(sky);

// Add a directional light to simulate the sun. LightSourceEntity sun = new LightSourceEntity(); sun.State.Name = “Sun”;

sun.Type = LightSourceEntityType.Directional; sun.Color = new Vector4(1, 1, 1, 1);

sun.Direction = new Vector3(-0.47f, -0.8f, -0.36f); SimulationEngine.GlobalInstancePort.Insert(sun);

Adding a Sky, Sun, and Ground

You use a typical sky and sun:

HeightFieldShapeProperties hf = new HeightFieldShapeProperties(“height field”, 64, // number of rows

10, // distance in meters, between rows 64, // number of columns

10, // distance in meters, between columns

340

www.it-ebooks.info

Chapter 7: Using Orchestration Services to Build a Simulation Scenario

1, // scale factor to multiple height values -1000); // vertical extent of the height field.

// create array with height samples

hf.HeightSamples = new HeightFieldSample[hf.RowCount * hf.ColumnCount]; for (int i = 0; i < hf.RowCount * hf.ColumnCount; i++)

{

hf.HeightSamples[i] = new HeightFieldSample(); hf.HeightSamples[i].Height = (short)(Math.Sin(i * 0.01));

}

// create a material for the entire field.

hf.Material = new MaterialProperties(“ground”, 0.8f, 0.5f, 0.8f);

// insert ground entity in simulation and specify a texture SimulationEngine.GlobalInstancePort.Insert(

new HeightFieldEntity(hf, “FieldGrass.dds”));

You use a ground plane substantially smaller than the standard flat ground used in the samples. This one is only 640 meters by 640 meters. Add the FieldGrass.dds texture to give the appearance of an open grassy field.

Adding Barriers

Now that you have the basics in place, you need to add a few barriers to make life interesting for your Corobot. The following code adds several walls and a cement tower. By grouping the barrier parameters in an array, it is easier to add additional barriers or define several different scene configurations. If you are trying to create a general-purpose navigation algorithm, you probably want to test it against several different barrier configurations.

Add the following code to create and insert the barriers just after the ground definition in

PopulateWorld:

// create barriers

foreach (Barrier bar in Barriers)

{

SingleShapeEntity wall = new SingleShapeEntity(

new BoxShape(

new BoxShapeProperties(

0, // no mass makes a static shape new Pose(),

bar.Dimensions)), // dimensions bar.Position);

wall.State.Pose.Orientation = bar.Orientation; wall.State.Name = bar.Name; wall.State.Assets.DefaultTexture = bar.Texture; SimulationEngine.GlobalInstancePort.Insert(wall);

}

341

www.it-ebooks.info

Part II: Simulations

Add this class definition as a peer to the MagellanReferee class:

public struct Barrier

{

public string Name; public Vector3 Position; public Vector3 Dimensions; public string Texture;

public Quaternion Orientation;

public Barrier(string name, Vector3 position, Vector3 dimensions, string texture, Quaternion orientation)

{

Name = name; Position = position;

Dimensions = dimensions; Texture = texture; Orientation = orientation;

}

}

Finally, add the following barrier definitions as a member variable at the top of the

MagellanReferee class:

public Barrier[] Barriers = new Barrier[]

{

// Name, Position, Dimensions, Texture, Orientation

new Barrier(“Wall0”, new Vector3(0, 0, -4), new Vector3(4f, 0.8f, 0.1f), “BrickWall.dds”, new Quaternion(0,0,0,1)),

new Barrier(“Wall1”, new Vector3(-2.05f, 0, -3.05f), new Vector3(2f, 0.8f, 0.1f),

“BrickWall.dds”, Quaternion.FromAxisAngle(0, 1, 0, (float)(Math.PI / 2))), new Barrier(“Wall2”, new Vector3(1.41f, 0, 2.46f), new Vector3(6f, 0.8f, 0.1f), “BrickWall.dds”, Quaternion.FromAxisAngle(0, 1, 0, (float)(Math.PI / 2))),

new Barrier(“Tower”, new Vector3(5.58f, 2f, -0.59f), new Vector3(2f, 4f, 2f), “MayangConcrP.dds”, new Quaternion(0,0,0,1)),

};

Running the Service

It’s a good time to run the service again to ensure that everything looks right. Run the service using the instructions in the previous section and switch to the FromAbove camera using F8. You should see a layout something like what is shown in Figure 7-2, which shows the Sim-Magellan world with a few walls and a tower.

342