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

www.it-ebooks.info

Chapter 6: Extending the MRDS Visual Simulation Environment

_targetRotation = double.MaxValue; if (_driveDistancePort != null)

{

_driveDistancePort.Post(

new Exception(“Request superceded prior to completion.”)); _driveDistancePort = null;

}

if (_rotateDegreesPort != null)

{

_rotateDegreesPort.Post(

new Exception(“Request superceded prior to completion.”)); _rotateDegreesPort = null;

}

}

The Drive Methods

The DifferentialDriveEntity provides several methods that drive its two wheels. Your entity needs to implement these same methods with four wheels in mind. The DifferentialDriveEntity keeps track of a target velocity for each wheel. Each frame, the axle speed of each wheel is adjusted according to this target velocity. The Update method also adjusts the target speed of each wheel if a DriveDistance or RotateDegrees command is currently being executed. It attempts to slow the wheels as the target distance or heading approaches so that the target is not overshot.

Add the following Update method override to the CorobotEntity class:

const float SPEED_DELTA = 0.5f;

public override void Update(FrameUpdate update)

{

// update state from the physics engine PhysicsEntity.UpdateState(true);

This call updates the entity state Pose and Velocity from the physics engine:

if (_distanceToTravel > 0)

{

// DriveDistance update double currentDistance =

Vector3.Length(State.Pose.Position – _startPoseForDriveDistance.Position);

if (currentDistance >= _distanceToTravel)

{

_wheelFR.Wheel.AxleSpeed = 0; _wheelFL.Wheel.AxleSpeed = 0; _wheelRR.Wheel.AxleSpeed = 0; _wheelRL.Wheel.AxleSpeed = 0; _leftTargetVelocity = 0; _rightTargetVelocity = 0; _distanceToTravel = 0;

// now that we’re finished, post a response if (_driveDistancePort != null)

(continued)

293

www.it-ebooks.info

Part II: Simulations

(continued)

{

SuccessFailurePort tmp = _driveDistancePort; _driveDistancePort = null;

tmp.Post(new SuccessResult());

}

}

else

{

// need to drive further, check if we should slow down if (progressPoints.Count >= averageKernel)

{

double distanceRemaining = _distanceToTravel - currentDistance;

double framesToCompletion = distanceRemaining * averageKernel /

(currentDistance - progressPoints.Dequeue()); if (framesToCompletion < decelerateThreshold)

{

_leftTargetVelocity *= 0.5f; _rightTargetVelocity *= 0.5f; progressPoints.Clear();

}

}

progressPoints.Enqueue(currentDistance);

}

}

The preceding code handles the behavior of the entity while a DriveDistance command is being executed. Suffice it to say that the code attempts to slow the entity as it approaches the distance goal so that it doesn’t overshoot it. This is a better implementation than the one found in the DifferentialDriveEntity but there is still room for improvement:

else if (_targetRotation != double.MaxValue)

{

// RotateDegrees update

float currentHeading = CurrentHeading;

double angleDelta = currentHeading - _previousHeading; while (angleDelta > Math.PI)

angleDelta -= twoPI; while (angleDelta <= -Math.PI)

angleDelta += twoPI; _currentRotation += angleDelta;

_previousHeading = currentHeading; // for next frame

float angleError;

if (_targetRotation < 0)

angleError = (float)(_currentRotation - _targetRotation);

else

angleError = (float)(_targetRotation - _currentRotation);

if (angleError < acceptableRotationError)

294

www.it-ebooks.info

Chapter 6: Extending the MRDS Visual Simulation Environment

{

//current heading is within acceptableError or has overshot

//end the rotation

_targetRotation = double.MaxValue; _wheelFR.Wheel.AxleSpeed = 0; _wheelFL.Wheel.AxleSpeed = 0; _wheelRR.Wheel.AxleSpeed = 0; _wheelRL.Wheel.AxleSpeed = 0; _leftTargetVelocity = 0; _rightTargetVelocity = 0;

// now that we’re finished, post a response if (_rotateDegreesPort != null)

{

SuccessFailurePort tmp = _rotateDegreesPort; _rotateDegreesPort = null;

tmp.Post(new SuccessResult());

}

}

else

{

if (angleDelta != 0)

{

// need to turn more, check if we should slow down if (progressPoints.Count >= averageKernel)

{

double framesToCompletion = Math.Abs(angleError * averageKernel /

(_currentRotation - progressPoints.Dequeue())); if (framesToCompletion < decelerateThreshold)

{

_leftTargetVelocity *= 0.5f; _rightTargetVelocity *= 0.5f; progressPoints.Clear();

}

}

progressPoints.Enqueue(_currentRotation);

}

}

}

The following code handles the RotateDegrees command. Just like DriveDistance, it attempts to slow the rotation of the entity as the target heading approaches:

float left = _wheelFL.Wheel.AxleSpeed + _leftTargetVelocity; float right = _wheelFR.Wheel.AxleSpeed + _rightTargetVelocity;

if (Math.Abs(left) > 0.1)

{

if (left > 0)

_wheelFL.Wheel.AxleSpeed -= SPEED_DELTA;

else

_wheelFL.Wheel.AxleSpeed += SPEED_DELTA;

}

(continued)

295

www.it-ebooks.info

Part II: Simulations

(continued)

if (Math.Abs(right) > 0.1)

{

if (right > 0)

_wheelFR.Wheel.AxleSpeed -= SPEED_DELTA;

else

_wheelFR.Wheel.AxleSpeed += SPEED_DELTA;

}

The AxleSpeed is the negative of the target velocity. When the two are nearly equal, left and right will be close to zero. If they are not nearly equal, then the axle speed is adjusted by SPEED_DELTA.

Here, the four-wheel-drive is implemented. The rear wheels are given axle speeds comparable to the front wheels:

// match the rear wheels with the front wheels _wheelRL.Wheel.AxleSpeed = _wheelFL.Wheel.AxleSpeed; _wheelRR.Wheel.AxleSpeed = _wheelFR.Wheel.AxleSpeed;

Finally, the Update method for the wheel entities is called, along with the base Update method:

//update entities in fields _wheelFL.Update(update); _wheelFR.Update(update); _wheelRL.Update(update); _wheelRR.Update(update);

//sim engine will update children base.Update(update);

}

The SimulatedQuadDifferentialDrive Service

Now you know how to create a custom simulation entity, but let’s face it: it’s pretty boring. It just sits there doing nothing. Wouldn’t it be great to be able to drive it around in the simulation environment? That is the topic of this next section.

A SimulatedDifferentialDrive service is provided with the MRDS SDK. It can be controlled with the SimpleDashboard service and it drives two-wheeled robots that subclass the DifferentialDriveEntity class. However, you have a four-wheeled robot that doesn’t use

the DifferentialDriveEntity class, so you are going to build a custom service that supports the same generic drive contract, enabling you to use the SimpleDashboard to control it.

Just as you previously used DssNewService.exe to create the Corobot service, you now use it to create your SimulatedQuadDifferentialDrive service. However, there is a twist this time. You’re going to tell DssNewService that you want the new service to support the generic drive contract as an alternate contract. This means that your service will support two different ports, each identified with a different contract. The alternate port will look just like the port that the SimulatedDifferentialDrive service supports.

296

www.it-ebooks.info

Chapter 6: Extending the MRDS Visual Simulation Environment

Go back to the MyChapter6 directory and use the following command line (shown in bold below) to create the SimulatedQuadDifferentialDrive service. Each command-line option to dssnewservice is shown on a separate line due to word wrap, but you should type them all on one line. The bold code indicates text that you should type:

C:\Microsoft Robotics Studio (1.5)\ProMRDS\MyChapter6>dssnewservice

/s:SimulatedQuadDifferentialDrive

/i:”\Microsoft Robotics Studio (1.5)\bin\RoboticsCommon.dll” /alt:”http://schemas.microsoft.com/robotics/2006/05/drive.html” /Namespace:”ProMRDS.Simulation.QuadDifferentialDrive”

/year:”2007” /month:”07”

The /s parameter names the new service. The /alt parameter specifies the contract that you want to implement as an alternate contract, and the /i parameter specifies where that contract is implemented. You use the /year and /month parameters to ensure that the contract for this service will be the same as the SimulatedQuadDifferentialDrive service in the Chapter6 directory.

Take a moment to look at the service code that has been generated. DssNewService has generated a service call SimulatedQuadDifferentialDrive service that supports an alternate contract identified by

“http://schemas.microsoft.com/robotics/2006/05/drive.html”. This is the generic drive contract you specify on the command line. It has defined a _mainPort of type pxdrive.DriveOperations and a service state of type pxdrive.DriveDifferentialTwoWheelState. You’re going to shuffle things a bit because you want to support two ports: an alternate port that supports the generic drive contract, and the main port that supports the new SimulatedQuadDifferentialDrive contract.

Begin modifying SimulatedQuadDifferentialDrive.cs by replacing the using statements at the top with the following to prepare you to access the simulator and the Corobot entity you just defined:

using Microsoft.Ccr.Core; using Microsoft.Dss.Core;

using Microsoft.Dss.Core.Attributes; using Microsoft.Dss.ServiceModel.Dssp;

using Microsoft.Dss.ServiceModel.DsspServiceBase; using Microsoft.Dss.Services.SubscriptionManager; using System;

using W3C.Soap;

using System.Collections.Generic;

using dssphttp = Microsoft.Dss.Core.DsspHttp;

using pxdrive = Microsoft.Robotics.Services.Drive.Proxy; using xml = System.Xml;

using xna = Microsoft.Xna.Framework;

using submgr = Microsoft.Dss.Services.SubscriptionManager; using simtypes = Microsoft.Robotics.Simulation;

using simengine = Microsoft.Robotics.Simulation.Engine; using physics = Microsoft.Robotics.Simulation.Physics; using corobot = ProMRDS.Simulation.Corobot;

using Microsoft.Robotics.PhysicalModel;

The new project will already have a reference to RoboticsCommon.Proxy.dll because it is referencing the generic drive contract from that DLL. You’ll also need to add references to the following DLLs.

297

www.it-ebooks.info

Part II: Simulations

Don’t forget to edit the properties of each DLL you add to set Copy Local and Specific Version to

False:

Corobot.Y2007.M07.dll

Microsoft.Xna.Framework.dll

PhysicsEngine.dll

RoboticsCommon.dll

SimulationCommon.dll

SimulationEngine.dll

You may be wondering why you need a reference to RoboticsCommon.dll when the project already has a reference to RoboticsCommon.proxy.dll. The definition of the generic drive contract is drawn from the proxy DLL, and the common types such as Vector3 are drawn from RoboticsCommon.dll.

Now you add a port to receive the generic drive commands:

// Port for receiving generic differential drive commands [AlternateServicePort(

AllowMultipleInstances = true,

AlternateContract = pxdrive.Contract.Identifier)] private pxdrive.DriveOperations _diffDrivePort = new

Microsoft.Robotics.Services.Drive.Proxy.DriveOperations();

Change the definition of _mainPort to support the extended quad drive operations:

///<summary>

///Main service port for quad drive commands

///</summary> [ServicePort(“/simulatedquaddifferentialdrive”,

AllowMultipleInstances = true)]

private QuadDriveOperations _mainPort = new QuadDriveOperations();

You also need to extend the state for this service because you need to keep track of four wheels now, rather than two. Replace the _state declaration with the following:

[InitialStatePartner( Optional = true,

ServiceUri = “SimulatedQuadDifferentialDriveService.Config.xml”)] private DriveDifferentialFourWheelState _state =

new DriveDifferentialFourWheelState();

You’ll also add the following definition of this class in SimulatedQuadDifferentialDriveTypes.cs just after the Contract class definition:

public class DriveDifferentialFourWheelState : pxdrive. DriveDifferentialTwoWheelState

{

private pxmotor.WheeledMotorState _rearLeftWheel; private pxmotor.WheeledMotorState _rearRightWheel; private Vector3 _position;

[Description(“The rear left wheel’s state.”)] [DataMember]

298

www.it-ebooks.info

Chapter 6: Extending the MRDS Visual Simulation Environment

public pxmotor.WheeledMotorState RearLeftWheel

{

get { return _rearLeftWheel; } set { _rearLeftWheel = value; }

}

[DataMember]

[Description(“The rear right wheel’s state.”)] public pxmotor.WheeledMotorState RearRightWheel

{

get { return _rearRightWheel; } set { _rearRightWheel = value; }

}

[DataMember]

[Description(“The current position of the entity.”)] public Vector3 Position

{

get { return _position; } set { _position = value; }

}

}

DriveDifferentialFourWheelState inherits from pxdrive.DriveDifferentialTwoWheelState, so all you need to do is add a definition for the rear wheels. You’ll also add the current position of the entity, which you’ll use as a crude way to simulate a GPS system later.

As long as you’re modifying SimulatedQuadDifferentialDriveTypes.cs, you may as well add the definition for QuadDriveOperations, which are the operations supported by the main port. It will support DsspDefaultLookup, DsspDefaultDrop, HttpGet, Get, and a new operation: SetPose. Add this code for the operations after the state definition:

///<summary>

///QuadDrive Operations Port

///</summary>

[ServicePort]

public class QuadDriveOperations : PortSet<DsspDefaultLookup,DsspDefaultDrop,HttpGet,Get,SetPose>

{

}

///<summary>

///Operation Retrieve Drive State

///</summary>

[Description(“Gets the drive’s current state.”)] public class Get :

Get<GetRequestType, PortSet<DriveDifferentialFourWheelState, Fault>>

{

}

///<summary>

///Operation Set Entity Pose

///</summary>

[Description(“Sets the pose of the quadDifferentialDrive entity.”)] public class SetPose :

Update<SetPoseRequestType, PortSet<DefaultUpdateResponseType, Fault>>

(continued)

299

www.it-ebooks.info

Part II: Simulations

(continued)

{

}

///<summary>

///Set entity pose request

///</summary> [DataMemberConstructor] [DataContract]

public class SetPoseRequestType

{

Pose _entityPose;

[DataMember]

public Pose EntityPose

{

get { return _entityPose; } set { _entityPose = value; }

}

public SetPoseRequestType()

{

}

}

Notice that you’ve defined a new operation, SetPose, which is an Update operation. It has a SetPoseRequestType class as its body, which in turn contains an EntityPose. Before you leave the file, replace the using statements at the top with the following:

using Microsoft.Ccr.Core;

using Microsoft.Dss.Core.Attributes; using Microsoft.Dss.ServiceModel.Dssp; using Microsoft.Dss.Core.DsspHttp; using System;

using System.Collections.Generic; using System.ComponentModel; using W3C.Soap;

using pxdrive = Microsoft.Robotics.Services.Drive.Proxy; using pxmotor = Microsoft.Robotics.Services.Motor.Proxy; using Microsoft.Robotics.PhysicalModel;

To summarize, you’ve changed the _mainPort to be a QuadDriveOperations port and added another port called _diffDrivePort, which supports the generic drive operations. You also changed the service state to DriveDifferentialFourWheelState, which includes all of the generic drive state as well as two additional wheels and a Pose.

Simulation Entity Notifications

All simulation services have one thing in common: They need to manipulate or read data from entities in the simulation environment. This is easy if the service created and inserted the entity, as was the case with the Corobot service. The SimulatedQuadDifferentialDrive service needs to interact with the Corobot entity in the simulation environment, so it needs to request a notification from the

300

www.it-ebooks.info

Chapter 6: Extending the MRDS Visual Simulation Environment

SimulationEngine service when the entity it needs is inserted into the environment. It does this by sending a subscribe message to the SimulationEngine service, which includes the entity name and a port to receive the notification. To support this, add the following variables at the top of

the SimulatedQuadDifferentialDriveService class definition in

SimulatedQuadDifferentialDrive.cs:

#region Simulation Variables corobot.CorobotEntity _entity;

simengine.SimulationEnginePort _notificationTarget; #endregion

_entity will eventually hold a reference to the Corobot entity in the simulation environment, and _notificationTarget is the port that will receive the notification.

The subscribe message is sent to the simulator in the Start method even before the service calls base.Start to insert itself into the service directory:

protected override void Start()

{

if (_state == null) CreateDefaultState();

_notificationTarget = new simengine.SimulationEnginePort();

//PartnerType.Service is the entity instance name. simengine.SimulationEngine.GlobalInstancePort.Subscribe( ServiceInfo.PartnerList, _notificationTarget);

//don’t start listening to DSSP operations, other than drop,

//until notification of entity

Activate(new Interleave(

new TeardownReceiverGroup

(

Arbiter.Receive<simengine.InsertSimulationEntity>( false,

_notificationTarget, InsertEntityNotificationHandlerFirstTime),

Arbiter.Receive<DsspDefaultDrop>( false,

_mainPort, DefaultDropHandler),

Arbiter.Receive<DsspDefaultDrop>( false,

_diffDrivePort, DefaultDropHandler)

),

new ExclusiveReceiverGroup(), new ConcurrentReceiverGroup()

));

}

301

www.it-ebooks.info

Part II: Simulations

As a convenience, the Subscribe method on the simulation engine port takes a PartnerList as a parameter. The name of the entity is contained in the partner list because it is specified as a partner to the SimulatedQuadDifferentialDrive service in the manifest. Update the manifest line at the top of the Corobot manifest to create a simcommon namespace to save typing:

<Manifest xmlns=”http://schemas.microsoft.com/xw/2004/10/manifest.html” xmlns:dssp=”http://schemas.microsoft.com/xw/2004/10/dssp.html”

xmlns:simcommon=”http://schemas.microsoft.com/robotics/2006/04/simulation.html”

>

Add the following lines to the Corobot manifest to start the drive service and to specify the entity named “Corobot” as a partner. This ServiceRecordType is inserted immediately after the

</ServiceRecordType> line that ends the Corobot service record. ProMRDS\config\Corobot

.manifest.xml provides an example.

<ServiceRecordType>

<dssp:Contract>http://schemas.tempuri.org/2007/07/simulatedquaddifferentialdrive

.html</dssp:Contract> <dssp:PartnerList> <dssp:Partner>

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

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

</ServiceRecordType>

The entity name is in the form of a URI, so an entity name of “Corobot” becomes “http://localhost/ Corobot”.

When a partner is defined this way in the manifest, it shows up in the PartnerList and the simulation engine will parse the passed PartnerList until it finds an entity name. When it receives the subscribe request, it scans through all of the entities in the environment. If that entity already exists in the environment, then the simulation engine immediately sends a notification to the _notificationTarget port with a reference to the named entity. If the entity does not exist in the environment, then the simulation engine waits until it is inserted before sending the notification. If the entity is never inserted, then no notification is ever sent.

In the Start method, a new Interleave is activated, which activates handlers for the DsspDefaultDrop message on each port and a handler for an InsertSimulationEntity handler on the _notificationTarget port. No other messages are processed and the service won’t even show up in the service directory until it receives a notification from the simulation engine.

The next step is to add the InsertEntityNotificationHandlerFirstTime method, which handles the InsertSimulationEntity notification:

void InsertEntityNotificationHandlerFirstTime( simengine.InsertSimulationEntity ins)

302