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

www.it-ebooks.info

Chapter 7: Using Orchestration Services to Build a Simulation Scenario

}

public FromWinformMsg(MsgEnum command, string[] parameters)

{

_command = command; _parameters = parameters;

}

public FromWinformMsg(

MsgEnum command, string[] parameters, object objectParam)

{

_command = command; _parameters = parameters; _object = objectParam;

}

}

#endregion

Add the definitions of the FromWinformEvents class and the FromWinformMsg class to

SimMagellanTypes.cs. Add a member variable to the SimMagellanUI class in SimMagellan.cs to receive messages from the Windows Form:

// This port receives events from the user interface FromWinformEvents _fromWinformPort = new FromWinformEvents();

Now add a handler to the interleave to listen for messages from this new port in the Start method, just after the call to base.Start:

// Add the winform message handler to the interleave MainPortInterleave.CombineWith(new Interleave(

new TeardownReceiverGroup(), new ExclusiveReceiverGroup

(

Arbiter.Receive<FromWinformMsg>(

true, _fromWinformPort, OnWinformMessageHandler)

),

new ConcurrentReceiverGroup()

));

Instantiating a Windows Form is as simple as posting a message to the WinFormsServicePort defined by the CCR Winforms Adapter immediately after the interleave is set:

// Create the user interface form WinFormsServicePort.Post(new RunForm(CreateForm));

RunForm is a class defined by the Winforms Adapter that takes a delegate as a parameter. The delegate is responsible for instantiating the form. The adapter code starts a thread that is dedicated to the form and then uses that thread to call the passed delegate. In this case, the delegate is a method on the

SimMagellan class called CreateForm:

// Create the UI form System.Windows.Forms.Form CreateForm()

{

return new SimMagellanUI(_fromWinformPort);

}

353

www.it-ebooks.info

Part II: Simulations

You pass a reference to _fromWinformPort so that the form can use this port to pass back messages.

In the SimMagellanUI.cs file, you need to modify the SimMagellanUI class constructor to accept this parameter:

FromWinformEvents _fromWinformPort;

public SimMagellanUI(FromWinformEvents EventsPort)

{

_fromWinformPort = EventsPort; InitializeComponent(); _fromWinformPort.Post(

new FromWinformMsg(FromWinformMsg.MsgEnum.Loaded, null, this));

}

The constructor keeps a reference to this port in _fromWinformPort and then initializes itself and sends a message back to the SimMagellan service to indicate that the form is now loaded.

Back in the SimMagellanUI class, add a handler for this message as follows:

SimMagellanUI _magellanUI = null;

// process messages from the UI Form

void OnWinformMessageHandler(FromWinformMsg msg)

{

switch (msg.Command)

{

case FromWinformMsg.MsgEnum.Loaded: // the windows form is ready to go

_magellanUI = (SimMagellanUI)msg.Object; break;

}

}

When the Loaded message is received from the form, the service stores a reference to the form. This reference sends commands to the form.

Now you have established a way for the service to send commands and information to the form, and a way for the form to notify the service when user interface events occur.

You should take this opportunity to compile your SimMagellan service. Run it using the Manifest Editor as described in the section “Creating a Manifest with the Manifest Editor.” The form associated with the SimMagellan service should appear when the service runs.

How to Make a Robot Behave

The next task is to give the robot an objective and a way to accomplish that objective. In this case, the objective is fairly clear. Your robot needs to ask the referee service where the cones are, and then it needs to navigate to each one and touch it.

354

www.it-ebooks.info

Chapter 7: Using Orchestration Services to Build a Simulation Scenario

An common method for defining a behavior like this is to set up a state machine. A state machine defines several states that represent a current situation for the robot. Certain events from the outside world can change the state. For example, the robot could have a Wander state whereby it is moving about the environment trying to get closer to a cone. When the IR sensor detects an obstacle nearby, the state may change from Wander to AvoidCollision, and the SimMagellan service will issue commands to the Drive service to move the robot away from the obstacle. When the obstacle has been avoided, the robot state will change back to Wander.

In this section, you’ll set up the state machine for the robot and define the actions it takes in each of these states. It should be stated again that this behavior algorithm is by no means optimal. You’ve done a lot of work in this chapter to set up the scenario and you’ll have a basic behavior implemented to demonstrate it, but the whole purpose of the exercise is to provide a way to refine the behavioral algorithm and to prototype new ideas.

Defining Behavior States

You will be defining the following states. Each state listed here includes a brief description of the associated robot behavior:

NotSpecified: This is the initial state of the state machine. The robot does nothing at all in this state. It moves to the Reset state when the Reset button is pressed.

Reset: In this state, the robot begins processing camera frames, displaying the elapsed time on the user interface. It retrieves a list of the cone positions from the Referee service and subscribes to the IR Sensor service for notifications. It enables the robot drive and moves the robot to its initial position. After all this is done, the state is changed to Ready.

Ready: The Ready state is much like the NotSpecified state. The robot does nothing at all. It changes to the Wander state when the Start button is pressed.

Wander: This is the main state of the robot while it navigates in the environment. In this state, the robot checks the last image received from the camera. If it detects a cone, then the state changes to Approach. Otherwise, the robot uses a method called GetHappiness to determine its next move. This method returns a “happiness” value that is calculated according to the robot’s proximity to a cone that has not yet been touched. The idea is that the robot seeks higher and higher levels of happiness by navigating closer to a cone until it is close enough to see it

and move directly toward it. There is some degree of randomness associated with the movement in this state because set patterns of movement can sometimes result in endless cycles. If the happiness level in the current position is greater than the happiness level in the previous position, the robot continues on its current course. Otherwise, it backs up to its previous position, adjusts its heading randomly, and then proceeds forward in the next direction. This behavior is repeated until the robot moves into the Approach state or the AvoidCollision state.

AvoidCollision: This state is entered when the IR Sensor service notifies the SimMagellan service that the distance measurement has changed and is less than one meter. In this state, the robot backs away a quarter meter, turns 90 degrees to the left, and then moves forward one meter. The robot then returns to the Wander state.

Approach: When the robot sees a cone, it enters the Approach state. In this state, the robot checks the distance from the IR sensor. If it is less than a meter, it goes to the FinalApproach state. Otherwise, it checks the position of the cone in the camera frame, adjusts its heading to head straight for the cone, and then moves forward a quarter meter and repeats the process.

355

www.it-ebooks.info

Part II: Simulations

FinalApproach: By the time the robot reaches this state, it is closer than one meter to the cone and pointed more or less straight at the cone. In this mode, the robot no longer relies on the camera image but only on the IR distance data. It waits for another IR distance reading and then slowly moves that distance toward the cone. Then it marks the cone as having been touched, after which that cone is no longer considered in the GetHappiness method. After this is done, the robot enters the BackAway state.

BackAway: The purpose of this state is to allow the robot to back away from the cone it just touched. It moves backward one meter and then turns 45 degrees to the left and resumes wandering. If all of the cones are touched, then the robot enters the Finished state.

Finished: At this point, the robot has accomplished its objective. The elapsed time is no longer incremented on the user interface, and the robot spins in circles to show that it is victorious. It doesn’t leave this state unless the Reset button is pushed to restart the scenario.

Implementing the Behavior for Each State

Now that you understand the states and behaviors defined in the last section, you are ready to write the code to implement this state machine. The current robot state will be stored in the SimMagellan service state. Replace the definition of the SimMagellanState class in SimMagellanTypes.cs as follows:

[DataContract] public enum ModeType

{

NotSpecified = 0, Reset,

Ready,

Wander,

AvoidCollision,

Approach,

FinalApproach,

BackAway,

Finished

}

///<summary>

///The SimMagellan State

///</summary> [DataContract()]

public class SimMagellanState

{

[DataMember]

public ModeType CurrentMode;

}

Add a BehaviorLoop method to the SimMagellan class:

// keep track of the time to finish DateTime _startTime;

DateTime _endTime;

const float _rotateSpeed = 0.3f; const float _driveSpeed = 0.5f;

356

www.it-ebooks.info

Chapter 7: Using Orchestration Services to Build a Simulation Scenario

bool _iteratorsStarted = false;

IEnumerator<ITask> BehaviorLoop()

{

bool driveCommandFailed = false;

ModeType previousMode = ModeType.NotSpecified; while (true)

{

driveCommandFailed = false;

if (previousMode != _state.CurrentMode)

{

// update the UI WinFormsServicePort.FormInvoke(

delegate()

{

_magellanUI.SetCurrentState( _state.CurrentMode.ToString());

}

);

}

previousMode = _state.CurrentMode; switch (_state.CurrentMode)

{

case ModeType.NotSpecified: case ModeType.Ready:

yield return Arbiter.Receive( false,

TimeoutPort(100), delegate(DateTime timeout) { });

break;

}

}

}

For the previous block of code, the _driveSpeed and _rotateSpeed constants are used to set the default speed for the robot to drive and turn. _startTime and _endTime are used to keep track of how much time has elapsed since the Start button was pressed. The first thing the BehaviorLoop method does is to initialize the state to NotSpecified. It then enters an endless loop to continually process the robot’s behavior. Each time through the loop, it checks whether the mode has changed. If it has, you call the SetCurrentState API on the SimMagellanUI class to display the current state on the user interface. Notice that you do this by posting a FormInvoke message on the WinFormsServicePort with a delegate containing the code you want to run. This code is always executed on the Windows Forms thread.

A switch statement is used to take a specific action based on the current state. You’ve already implemented a behavior for two states: NotSpecified and Ready. The behavior for these states is to wait 100 ms and then break out of the switch statement to go through the loop again. The only way to break out of one of these states is to click the Reset or Start buttons.

Add a SpawnIterator call at the end of the Start method so that the BehaviorLoop begins after the service has initialized:

SpawnIterator(BehaviorLoop);

357

www.it-ebooks.info

Part II: Simulations

The next step is to add methods in the SimMagellanUI class that will process these button clicks and send a notification to the SimMagellan service. Add the following two event handlers to the

_resetButton and _startButton controls in the SimMagellanUI class:

private void _startButton_Click(object sender, EventArgs e)

{

_fromWinformPort.Post(

new FromWinformMsg(FromWinformMsg.MsgEnum.Start, null));

}

private void _resetButton_Click(object sender, EventArgs e)

{

_fromWinformPort.Post(

new FromWinformMsg(FromWinformMsg.MsgEnum.Reset, null)); _startButton.Enabled = true;

}

You also need to hook the Click event for each of the buttons in SimMagellanUI.designer.cs as shown here. If you double-click on the buttons in the designer, this is done for you:

this._resetButton.Click += new System.EventHandler(this._resetButton_Click); this._startButton.Click +=new System.EventHandler(this._startButton_Click);

Each of these handlers posts a message to the _fromWinformPort to tell the SimMagellan service that a button was clicked. While you’re modifying the SimMagellanUI class, you may as well add the methods that update the current state and the elapsed time labels on the form:

public void SetCurrentState(string state)

{

_stateLabel.Text = “State: “ + state;

}

public void SetElapsedTime(string time)

{

_elapsedLabel.Text = time;

}

Back in the SimMagellan class, add the following two case statements to the OnWinformMessageHandler to process these messages from the user interface:

case FromWinformMsg.MsgEnum.Reset: _state.CurrentMode = ModeType.Reset; break;

case FromWinformMsg.MsgEnum.Start: if(_state.CurrentMode == ModeType.Ready)

{

_startTime = DateTime.Now; _state.CurrentMode = ModeType.Wander;

}

break;

358

www.it-ebooks.info

Chapter 7: Using Orchestration Services to Build a Simulation Scenario

At this point, you should be able to compile the SimMagellan service and run it. When you click the Reset button, the State label should change to Reset.

Now you’re ready to implement the behavior for the Reset state. Add the following case statement to the BehaviorLoop method:

case ModeType.Reset:

{

if (!_iteratorsStarted)

{

// start processing camera frames SpawnIterator<DateTime>(DateTime.Now, ProcessFrame);

// start displaying elapsed time SpawnIterator<DateTime>(DateTime.Now, UpdateElapsedTime);

_irSensorPort.Subscribe(_irNotifyPort);

MainPortInterleave.CombineWith(new Interleave( new TeardownReceiverGroup(),

new ExclusiveReceiverGroup(), new ConcurrentReceiverGroup

(

Arbiter.Receive<irsensor.Replace>( true,

_irNotifyPort, IRNotifyReplaceHandler)

)

));

//we don’t want to start additional iterators if the reset

//button is pushed to restart the scenario

_iteratorsStarted = true;

}

yield return Arbiter.Choice(_refereePort.Get(), delegate(referee.MagellanRefereeState state)

{ SetWaypoints(state); }, delegate(Fault f) { }

);

_drivePort.EnableDrive(true); _drivePort.SetDriveSpeed(0, 0); _quadDrivePort.SetPose(

new quadDrive.SetPoseRequestType(

new Microsoft.Robotics.PhysicalModel.Proxy.Pose())); _state.CurrentMode = ModeType.Ready;

break;

}

Several things are going on here to get our little robot ready to start. First, you start the iterator methods that process the camera frames and update the elapsed time. You’ll look at those in just a bit.

359

www.it-ebooks.info

Part II: Simulations

Next, you subscribe to notifications from the IR Sensor service. This means that each time the value of the front IR sensor changes, the _irNotifyPort receives a message with the new IR sensor service state. You add a handler to the interleave to handle this message.

Next, you post a Get message to the Referee service to get a list of cone positions. You keep track of these positions in a list of waypoints. A waypoint holds the position of a cone and a Boolean indicating whether the cone has been visited.

Finally, the drive is enabled and a SetPose message is sent to the _quadDrivePort to move the robot back to the starting position. The default Pose that is sent has a position of (0,0,0) and an orientation with no rotations.

Let’s look at the SetWaypoints method first because it is the simplest:

private void SetWaypoints(referee.MagellanRefereeState state)

{

foreach (Vector3 location in state.Cones) _waypoints.Add(new Waypoint(location));

}

The _waypoints member is just a list of Waypoint objects, which are defined as follows:

class Waypoint

{

public Vector3 Location; public bool Visited;

public Waypoint(Vector3 location)

{

Location = location; Visited = false;

}

}

Declare _waypoints in the SimMagellan class as follows:

List<Waypoint> _waypoints = new List<Waypoint>();

Now you need to add the UpdateElapsedTime method. This method runs in an endless loop to determine the elapsed time depending on the current state. If the string representing the time has changed since the last time it has been displayed, it is sent to the UI with a call to

_magellanUI.SetElapsedTime:

private IEnumerator<ITask> UpdateElapsedTime(DateTime timeout)

{

string previous = string.Empty; while (true)

{

string newString = string.Empty;

switch (_state.CurrentMode)

360

www.it-ebooks.info

Chapter 7: Using Orchestration Services to Build a Simulation Scenario

{

case ModeType.NotSpecified: case ModeType.Ready:

case ModeType.Reset:

newString = string.Format(“Elapsed: {0}:{1:D2}”, 0, 0); break;

case ModeType.Wander: case ModeType.Approach:

case ModeType.FinalApproach: case ModeType.BackAway:

{

TimeSpan elapsed = DateTime.Now - _startTime; newString = string.Format(“Elapsed: {0}:{1:D2}”,

elapsed.Minutes, elapsed.Seconds); break;

}

case ModeType.Finished:

{

TimeSpan elapsed = _endTime - _startTime; newString = string.Format(“Elapsed: {0}:{1:D2}”,

elapsed.Minutes, elapsed.Seconds); break;

}

}

if (newString != previous)

{

previous = newString; // update the UI

WinFormsServicePort.FormInvoke(

delegate()

{

_magellanUI.SetElapsedTime(newString);

}

);

}

yield return Arbiter.Receive(

false, TimeoutPort(100), delegate { });

}

}

The handler for the IR Distance Sensor aborts any current drive requests by posting an AllStop message to the _drivePort if the distance reading is less than one meter. Any DriveDistance or RotateDegrees operations in progress will return a Fault response, which aborts the current operation and causes the state machine to begin processing the AvoidCollision state:

void IRNotifyReplaceHandler(irsensor.Replace replace)

{

if (_state.CurrentMode == ModeType.Wander)

(continued)

361

www.it-ebooks.info

Part II: Simulations

(continued)

{

//stop dead in our tracks, abort DriveDistance and

//RotateDegrees in progress

_drivePort.AllStop();

_state.CurrentMode = ModeType.AvoidCollision;

}

}

}

Processing Camera Frames

The last method to implement to make the Reset state complete is ProcessFrame. Because this method is a little more complicated, we’ll take a closer look at it.

The topic of robot vision and navigating using vision could fill many books, and it is still a largely unsolved problem. Fortunately, your requirements are simple, so the vision algorithm doesn’t need to be very complicated.

Vision algorithms can be very difficult to debug, so it is useful to have the SimMagellan service provide some feedback about what it is doing. The ProcessFrame method is quite simple by itself. It sends a request to the SimulatedWebCam service each 200 milliseconds:

private IEnumerator<ITask> ProcessFrame(DateTime timeout)

{

while (true)

{

yield return Arbiter.Choice( _cameraPort.QueryFrame(), ValidateFrameHandler, DefaultFaultHandler);

yield return Arbiter.Receive(

false, TimeoutPort(200), delegate { });

}

}

The DefaultFaultHandler simply logs the fault in case the camera was unable to return a frame:

void DefaultFaultHandler(Fault fault)

{

LogError(fault);

}

The camera frame response is handled by the ValidateFrameHandler. This handler checks the timestamp on the camera frame and discards the image if it is older than one second. This prevents the robot from making decisions based on stale data in case something delays the camera from sending frames. The frame is analyzed with the ProcessImage method and is then sent to the user interface form along with the analysis results. The analysis results are used to draw a rectangle around recognized objects:

362