Wrox Programmer Forums

Need to download code?

View our list of code downloads.

Register | FAQ | Members List | Calendar | Search | Today's Posts | Mark Forums Read
BOOK: Professional Microsoft Robotics Studio ISBN: 978-0-470-14107-6
This is the forum to discuss the Wrox book Professional Microsoft Robotics Developer Studio by Kyle Johns, Trevor Taylor; ISBN: 9780470141076
Welcome to the p2p.wrox.com Forums.

You are currently viewing the BOOK: Professional Microsoft Robotics Studio ISBN: 978-0-470-14107-6 section of the Wrox Programmer to Programmer discussions. This is a community of tens of thousands of software programmers and website developers including Wrox book authors and readers. As a guest, you can read any forum posting. By joining today you can post your own programming questions, respond to other developersí questions, and eliminate the ads that are displayed to guests. Registration is fast, simple and absolutely free .
DRM-free e-books 300x50
Reply
 
Thread Tools Display Modes
  #1 (permalink)  
Old September 9th, 2008, 10:08 PM
Registered User
 
Join Date: Sep 2008
Location: , , .
Posts: 1
Thanks: 0
Thanked 0 Times in 0 Posts
Default Multiple Robot Support for SLAM

I am new to MRDS. I am wondering if the ExplorerSim application in chapter 9 is easily extendable to support multiple robots simultaneous trying to perform localization and mapping.

Reply With Quote
  #2 (permalink)  
Old September 13th, 2008, 01:41 AM
Wrox Author
 
Join Date: Apr 2008
Location: Brisbane, QLD, Australia.
Posts: 60
Thanks: 0
Thanked 5 Times in 5 Posts
Default

It is not "easily" extensible. This is one of the items on my "To Do" list.

Also, ExplorerSim does not currently do SLAM - it just draws a map.

And lastly, there is a new version of MRDS due soon so I probably will not make an updates until after it ships.

Reply With Quote
  #3 (permalink)  
Old February 24th, 2010, 10:43 AM
Registered User
 
Join Date: Feb 2010
Posts: 2
Thanks: 0
Thanked 0 Times in 0 Posts
Question Trying to Explore and Map with Multiple Robots

Ok,

First of all I'm sorry for doing a bigger post every time, but the times this post is being visited motivates me for explaining my improvements.

Eventhough you need to create more variables in ExplorerSimTypes and ExplorerSimStates, I'll recommend for everyone trying this, to create your own service and create two additional source items for types and states and copy everything from ExplorerSim types and states respectively (without the namespace, which is going to be your now of course =)). Beware of contract identifier in Types, which must be identical to your service contract in your service manifest. Copy also the explorersim.config to your service folder, and also type the same contract name.

After these, the main code I'm using is copied below. My main tasks now are: first try to optimize the code or maybe follow any given recommendation for making it faster so I can really process good rate of FPS; and second, intelligence in exploring.

Hope it helps if anyone doing something like this, and hope more if anyone can tell me how to make my namespace more efficient. =P

[code]
[Contract(Contract.Identifier)]
[DisplayName("Test1_3Pioneers_DisasterMaze")]
[Description("Test1_3Pioneers_DisasterMaze service (Multiple Pioneer robots exploring/wandering a zone and creating a map using LRF's)")]
class Test_1 : DsspServiceBase
{
// The explorer uses there constants to adjust speed and rotation of the differential drive.
// Change those values to match your robot.You could expose them on the explorers
// state type and make the service configurable.
#region Constants
/// Amount to backup when hitting an obstacle.
const int BackupDistance = -300; // mm
/// If an obstacle comes within thisdistance the robot stops moving.
const int ObstacleDistance = 800; // mm
/// If the robot is mapping and has this much open space ahead it stops mapping and enters the space.
const int SafeDistance = 1000; // mm
/// The width of the corridor that must be safe in order to go from mapping to moving.
const int CorridorWidthMapping = 350; // mm
/// The minimum free distance that is required to drive with max. velocity.
const int FreeDistance = 1500; // mm
/// The free distance that is required to drive with 1/2 max velocity.
const int AwareOfObstacleDistance = 1000; // mm
/// The max. velocity with which to move.
const int MaximumForwardVelocity = 600; // mm/sec
/// The width of the corridor in which obstacles effect velocity.
const int CorridorWidthMoving = 500; // mm
/// If no laser data is received within this time the robot stops.
const int WatchdogTimeout = 500; // msec
/// Interval between timer notifications.
const int WatchdogInterval = 100; // msec
#endregion

//Control Variables
#region Variables
//Variables for the 3 Pioneer entities
simengine.SimulationEnginePort _notificationTarget;
DifferentialDriveEntity _robot = null;
simengine.SimulationEnginePort _notificationTarget2;
DifferentialDriveEntity _robot2 = null;
simengine.SimulationEnginePort _notificationTarget3;
DifferentialDriveEntity _robot3 = null;

//Tracking Visited Points -Not Used Right Now
List<Waypoint> _waypoints = new List<Waypoint>();
List<Waypoint2> _waypoints2 = new List<Waypoint2>();
List<Waypoint3> _waypoints3 = new List<Waypoint3>();

//Happiness -Not Used Right Now
float _pHappiness = 0;
float _ppHappiness = 0;
float _escapeCounter = 0;
float _pHappiness2 = 0;
float _ppHappiness2 = 0;
float _escapeCounter2 = 0;
float _pHappiness3 = 0;
float _ppHappiness3 = 0;
float _escapeCounter3 = 0;
#endregion

//Start-up Behaviors
#region Initial Behaviors
//Starting triggered behavior for each robot
string _currentMode = "Ready";
string _currentMode2 = "Ready";
string _currentMode3 = "Ready";
#endregion

//Map Source Enablers
#region Specifying which laser contributes to mapping
bool lrfenable = true;
bool lrfenable2 = true;
bool lrfenable3 = true;
#endregion

/// <MSRDS Service state>
#region Service State
// TT - Added the InitialStatePartner attribute and changed
// _state to be null. This allows settings to be read from a config file.
[ServiceState]
[InitialStatePartner(Optional = true, ServiceUri = InitialStateUri)]
State _state = null;
private const string InitialStateUri = ServicePaths.MountPoint + @"/My_C#_Programs/config/ExplorerSim.Config.xml";
#endregion

/// <MSRDS Ports>
#region Ports
//MAIN Service Operations Port
[ServicePort("/Test1_3Pioneers_DisasterMaze", AllowMultipleInstances = true)]
TestOperations _mainPort = new TestOperations();
TestOperations _mainPort2 = new TestOperations();
TestOperations _mainPort3 = new TestOperations();
public Test_1(DsspServiceCreationPort create)
: base(create)
{
}

//Ports for Bumpers
bumper.ContactSensorArrayOperations _bumperPort = new bumper.ContactSensorArrayOperations();
bumper.ContactSensorArrayOperations _bumperNotifyPort = new bumper.ContactSensorArrayOperations();
bumper2.ContactSensorArrayOperations _bumperPort2 = new bumper2.ContactSensorArrayOperations();
bumper2.ContactSensorArrayOperations _bumperNotifyPort2 = new bumper2.ContactSensorArrayOperations();
bumper3.ContactSensorArrayOperations _bumperPort3 = new bumper3.ContactSensorArrayOperations();
bumper3.ContactSensorArrayOperations _bumperNotifyPort3 = new bumper3.ContactSensorArrayOperations();

//Ports for LRF's Scanners
//Detection Ports are specifically for PERCEPTIONS
//Notify Ports are specifically for MAPPING UPDATES
sicklrf.SickLRFOperations _lrfPort = new sicklrf.SickLRFOperations();
sicklrf.SickLRFOperations _lrfNotifyPort = new sicklrf.SickLRFOperations();
sicklrf.SickLRFOperations _lrfDetectionPort = new sicklrf.SickLRFOperations();
sicklrf2.SickLRFOperations _lrfPort2 = new sicklrf2.SickLRFOperations();
sicklrf2.SickLRFOperations _lrfNotifyPort2 = new sicklrf2.SickLRFOperations();
sicklrf.SickLRFOperations _lrfDetectionPort2 = new sicklrf.SickLRFOperations();
sicklrf3.SickLRFOperations _lrfPort3 = new sicklrf3.SickLRFOperations();
sicklrf3.SickLRFOperations _lrfNotifyPort3 = new sicklrf3.SickLRFOperations();
sicklrf.SickLRFOperations _lrfDetectionPort3 = new sicklrf.SickLRFOperations();

//Ports for Robots
dd.DriveOperations _drivePort = new dd.DriveOperations();
dd.DriveOperations _driveNotifyPort = new dd.DriveOperations();
dd2.DriveOperations _drivePort2 = new dd2.DriveOperations();
dd2.DriveOperations _driveNotifyPort2 = new dd2.DriveOperations();
dd3.DriveOperations _drivePort3 = new dd3.DriveOperations();
dd3.DriveOperations _driveNotifyPort3 = new dd3.DriveOperations();

#endregion

/// <MSRDS Start Method>
#region Start Method
protected override void Start()
{
//Variable initialization for 3 Pioneers
#region Robot Entity Notification Variables
_notificationTarget = new simengine.SimulationEnginePort();
_notificationTarget2 = new simengine.SimulationEnginePort();
_notificationTarget3 = new simengine.SimulationEnginePort();
#endregion

//Request to 3 Pioneers' platform building
#region Binding Robots
simengine.EntitySubscribeRequestType esrt = new simengine.EntitySubscribeRequestType();
esrt.Name = "p3dx1";
simengine.SimulationEngine.GlobalInstancePort.Subs cribe(esrt, _notificationTarget);
simengine.EntitySubscribeRequestType esrt2 = new simengine.EntitySubscribeRequestType();
esrt2.Name = "p3dx2";
simengine.SimulationEngine.GlobalInstancePort.Subs cribe(esrt2, _notificationTarget2);
simengine.EntitySubscribeRequestType esrt3 = new simengine.EntitySubscribeRequestType();
esrt3.Name = "p3dx3";
simengine.SimulationEngine.GlobalInstancePort.Subs cribe(esrt3, _notificationTarget3);
#endregion

//Start simulation engine from the state file
#region Loaded Environment
string filename = "SimState/disastermazenpioneers.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 });
#endregion

// TT - Make sure that we have a state in case no config file
if (_state == null)
CreateState();

// This part consists in the needed interleaves for obtaining a good mapping from pose and LRF.
#region Main Port Map Messaging
base.Start();
#region notification handler setup
MainPortInterleave.CombineWith(
Arbiter.Interleave(
new TeardownReceiverGroup(Arbiter.Receive<DsspDefaultD rop>(false, _mainPort, DropHandler)),
new ExclusiveReceiverGroup(),
new ConcurrentReceiverGroup(Arbiter.Receive<Get>(true, _mainPort, GetHandler),
Arbiter.Receive<dssp.DsspDefaultLookup>(true, _mainPort, DefaultLookupHandler))
)
);
if (lrfenable)
{
MainPortInterleave.CombineWith(
Arbiter.Interleave(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup(
Arbiter.Receive<LaserRangeFinderResetUpdate>(true, _mainPort, LaserRangeFinderResetUpdateHandler),
Arbiter.Receive<LaserRangeFinderUpdate>(true, _mainPort, LaserRangeFinderUpdateHandler)
),
new ConcurrentReceiverGroup()
)
);
}
if (lrfenable2)
{
MainPortInterleave.CombineWith(
Arbiter.Interleave(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup(
Arbiter.Receive<LaserRangeFinderResetUpdate>(true, _mainPort2, LaserRangeFinderResetUpdateHandler2),
Arbiter.Receive<LaserRangeFinderUpdate>(true, _mainPort2, LaserRangeFinderUpdateHandler2)
),
new ConcurrentReceiverGroup()
)
);
}
if (lrfenable3)
{
MainPortInterleave.CombineWith(
Arbiter.Interleave(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup(
Arbiter.Receive<LaserRangeFinderResetUpdate>(true, _mainPort3, LaserRangeFinderResetUpdateHandler3),
Arbiter.Receive<LaserRangeFinderUpdate>(true, _mainPort3, LaserRangeFinderUpdateHandler3)
),
new ConcurrentReceiverGroup()
)
);
}

MainPortInterleave.CombineWith(
Arbiter.Interleave(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup(),
new ConcurrentReceiverGroup(
Arbiter.Receive<sicklrf.Reset>(true, _lrfNotifyPort, LaserResetNotification),
Arbiter.Receive<sicklrf2.Reset>(true, _lrfNotifyPort2, LaserResetNotification2),
Arbiter.Receive<sicklrf3.Reset>(true, _lrfNotifyPort3, LaserResetNotification3)
)
)
);

Activate(
Arbiter.ReceiveWithIterator<sicklrf.Replace>(false , _lrfNotifyPort, LaserReplaceNotification),
Arbiter.ReceiveWithIterator<sicklrf2.Replace>(fals e, _lrfNotifyPort2, LaserReplaceNotification2),
Arbiter.ReceiveWithIterator<sicklrf3.Replace>(fals e, _lrfNotifyPort3, LaserReplaceNotification3)
);
#endregion
// SUBSCRIPTIONS FROM PORTS TO NOTIFY PORTS FOR EACH DRIVE, BUMPER AND LASER
// ARE DONE INSIDE CONNECTIONS.
#endregion

//Main Port Interleaving for binding 3 Pioneers if Existing
#region Robot Entity Binds Messaging
MainPortInterleave.CombineWith(
Arbiter.Interleave(
new TeardownReceiverGroup(),
new ExclusiveReceiverGroup(
Arbiter.Receive<simengine.InsertSimulationEntity>( true, _notificationTarget, InsertEntityNotificationHandler),
Arbiter.Receive<simengine.DeleteSimulationEntity>( true, _notificationTarget, DeleteEntityNotificationHandler),
Arbiter.Receive<simengine.InsertSimulationEntity>( true, _notificationTarget2, InsertEntityNotificationHandler2),
Arbiter.Receive<simengine.DeleteSimulationEntity>( true, _notificationTarget2, DeleteEntityNotificationHandler2),
Arbiter.Receive<simengine.InsertSimulationEntity>( true, _notificationTarget3, InsertEntityNotificationHandler3),
Arbiter.Receive<simengine.DeleteSimulationEntity>( true, _notificationTarget3, DeleteEntityNotificationHandler3)
),
new ConcurrentReceiverGroup()
)
);
#endregion

} //END OF START() METHOD

#endregion

//Mapping Methods
#region Mapping Handlers Methods
// Create the default state
void CreateState()
{
_state = new State();
// Map dimensions in meters
_state.MapWidth = 70;
_state.MapHeight = 70;
// Map resolution in meters
_state.MapResolution = 0.05;
// Maximum range in meters
_state.MapMaxRange = 7.99;
_state.BayesVacant = (byte)MapValues.VACANT;
_state.BayesObstacle = (byte)MapValues.OBSTACLE;
// Save the state now as a convenience to the user
//SaveState(_state);
}

#region DSS operation handlers (Get, Drop)
public void DropHandler(DsspDefaultDrop drop)
{
Console.WriteLine("I've dropped !");
SpawnIterator(drop, DoDrop);
}
IEnumerator<ITask> DoDrop(DsspDefaultDrop drop)
{
if (_mapForm != null)
{
MapForm map = _mapForm;
_mapForm = null;

WinFormsServicePort.FormInvoke(
delegate()
{
if (!map.IsDisposed)
{
map.Close();
map.Dispose();
}
}
);
}

if (_state.IsActive)
{
LogInfo("Service is being dropped while moving, Requesting Stop.");

yield return Arbiter.Choice(
DisableMotor(),
delegate(DefaultUpdateResponseType response) { },
delegate(Fault fault) { }
);
}

LogInfo("Dropping Test...");

base.DefaultDropHandler(drop);
}
void GetHandler(Get get)
{
get.ResponsePort.Post(_state);
}
#endregion

//Robots' Pose Updates for Mapping Variables
void DriveUpdate()
{
if (_robot != null)
{
float w = _robot.State.Pose.Orientation.W;
_state.X = _robot.State.Pose.Position.X;
_state.Y = _robot.State.Pose.Position.Z;
_state.Theta = (float)(Math.Acos(w) * 2 * 180 / Math.PI);
if (_robot.State.Pose.Orientation.Y < 0)
_state.Theta = -_state.Theta;
}
}
void DriveUpdate2()
{
if (_robot2 != null)
{
float w = _robot2.State.Pose.Orientation.W;
_state.X2 = _robot2.State.Pose.Position.X;
_state.Y2 = _robot2.State.Pose.Position.Z;
_state.Theta2 = (float)(Math.Acos(w) * 2 * 180 / Math.PI);
if (_robot2.State.Pose.Orientation.Y < 0)
_state.Theta2 = -_state.Theta2;
}
}
void DriveUpdate3()
{
if (_robot3 != null)
{
float w = _robot3.State.Pose.Orientation.W;
_state.X3 = _robot3.State.Pose.Position.X;
_state.Y3 = _robot3.State.Pose.Position.Z;
_state.Theta3 = (float)(Math.Acos(w) * 2 * 180 / Math.PI);
if (_robot3.State.Pose.Orientation.Y < 0)
_state.Theta3 = -_state.Theta3;
}
}

#region Laser handlers
//ROBOT 1:
/// <summary>
/// Handles Replace notifications from the Laser partner
/// </summary>
/// <remarks>Posts a <typeparamref name="LaserRangeFinderUpdate"/> to itself.</remarks>
/// <param name="replace">notification</param>
/// <returns>task enumerator</returns>
IEnumerator<ITask> LaserReplaceNotification(sicklrf.Replace replace)
{
// When this handler is called a couple of notifications may
// have piled up. We only want the most recent one.
sicklrf.State laserData = GetMostRecentLaserNotification(replace.Body);
LaserRangeFinderUpdate request = new LaserRangeFinderUpdate(laserData);
_mainPort.Post(request);
yield return Arbiter.Choice(
request.ResponsePort,
delegate(DefaultUpdateResponseType response) { },
delegate(Fault fault) { }
);
// Skip messages that have been queued up in the meantime.
// The notification that are lingering are out of data by now.
GetMostRecentLaserNotification(laserData);
// Reactivate the handler.
Activate(
Arbiter.ReceiveWithIterator<sicklrf.Replace>(false , _lrfNotifyPort, LaserReplaceNotification)
);
}
/// <summary>
/// Handles the <typeparamref name="LaserRangeFinderUpdate"/> request.
/// </summary>
/// <param name="update">request</param>
void LaserRangeFinderUpdateHandler(LaserRangeFinderUpda te update)
{
sicklrf.State laserData = update.Body;
_state.PreviousLaser = _state.MostRecentLaser;
_state.MostRecentLaser = laserData.TimeStamp;
TimeSpan ts = _state.MostRecentLaser - _state.PreviousLaser;
int interval = ts.Milliseconds;
if (_state.DrawMap)
SpawnIterator(laserData.AngularRange, laserData.DistanceMeasurements, UpdateMap);
update.ResponsePort.Post(DefaultUpdateResponseType .Instance);
}
/// <summary>
/// Gets the most recent laser notification. Older notifications are dropped.
/// </summary>
/// <param name="laserData">last known laser data</param>
/// <returns>most recent laser data</returns>
private sicklrf.State GetMostRecentLaserNotification(sicklrf.State laserData)
{
sicklrf.Replace testReplace;
int count = _lrfNotifyPort.P3.ItemCount - 1;
for (int i = 0; i < count; i++)
{
testReplace = _lrfNotifyPort.Test<sicklrf.Replace>();
if (testReplace.Body.TimeStamp > laserData.TimeStamp)
{
laserData = testReplace.Body;
}
}

if (count > 0)
{
LogInfo(string.Format("Dropped {0} laser readings (laser start)", count));
}
return laserData;
}
/// <summary>
/// Handles the reset notification of the Laser partner.
/// </summary>
/// <remarks>Posts a <typeparamref name="LaserRangeFinderResetUpdate"/> to itself.</remarks>
/// <param name="reset">notification</param>
void LaserResetNotification(sicklrf.Reset reset)
{
_mainPort.Post(new LaserRangeFinderResetUpdate(reset.Body));
}
/// <summary>
/// Handle the <typeparamref name="LaserRangeFinderResetUpdate"/> request.
/// </summary>
/// <remarks>Stops the robot.</remarks>
/// <param name="update">request</param>
void LaserRangeFinderResetUpdateHandler(LaserRangeFinde rResetUpdate update)
{
update.ResponsePort.Post(DefaultUpdateResponseType .Instance);

}

//ROBOT 2:
/// <summary>
/// Handles Replace notifications from the Laser partner
/// </summary>
/// <remarks>Posts a <typeparamref name="LaserRangeFinderUpdate"/> to itself.</remarks>
/// <param name="replace">notification</param>
/// <returns>task enumerator</returns>
IEnumerator<ITask> LaserReplaceNotification2(sicklrf2.Replace replace)
{
// When this handler is called a couple of notifications may
// have piled up. We only want the most recent one.
sicklrf2.State laserData = GetMostRecentLaserNotification2(replace.Body);
LaserRangeFinderUpdate request = new LaserRangeFinderUpdate(laserData);
_mainPort2.Post(request);
yield return Arbiter.Choice(
request.ResponsePort,
delegate(DefaultUpdateResponseType response) { },
delegate(Fault fault) { }
);
// Skip messages that have been queued up in the meantime.
// The notification that are lingering are out of data by now.
GetMostRecentLaserNotification2(laserData);
// Reactivate the handler.
Activate(
Arbiter.ReceiveWithIterator<sicklrf2.Replace>(fals e, _lrfNotifyPort2, LaserReplaceNotification2)
);
}
/// <summary>
/// Handles the <typeparamref name="LaserRangeFinderUpdate"/> request.
/// </summary>
/// <param name="update">request</param>
void LaserRangeFinderUpdateHandler2(LaserRangeFinderUpd ate update)
{
sicklrf2.State laserData = update.Body;
_state.PreviousLaser2 = _state.MostRecentLaser2;
_state.MostRecentLaser2 = laserData.TimeStamp;
TimeSpan ts = _state.MostRecentLaser2 - _state.PreviousLaser2;
int interval = ts.Milliseconds;
if (_state.DrawMap)
SpawnIterator(laserData.AngularRange, laserData.DistanceMeasurements, UpdateMap2);
update.ResponsePort.Post(DefaultUpdateResponseType .Instance);
}
/// <summary>
/// Gets the most recent laser notification. Older notifications are dropped.
/// </summary>
/// <param name="laserData">last known laser data</param>
/// <returns>most recent laser data</returns>
private sicklrf2.State GetMostRecentLaserNotification2(sicklrf2.State laserData)
{
sicklrf2.Replace testReplace;
int count = _lrfNotifyPort2.P3.ItemCount - 1;
for (int i = 0; i < count; i++)
{
testReplace = _lrfNotifyPort2.Test<sicklrf2.Replace>();
if (testReplace.Body.TimeStamp > laserData.TimeStamp)
{
laserData = testReplace.Body;
}
}

if (count > 0)
{
LogInfo(string.Format("Dropped {0} laser readings (laser start)", count));
}
return laserData;
}
/// <summary>
/// Handles the reset notification of the Laser partner.
/// </summary>
/// <remarks>Posts a <typeparamref name="LaserRangeFinderResetUpdate"/> to itself.</remarks>
/// <param name="reset">notification</param>
void LaserResetNotification2(sicklrf2.Reset reset)
{
_mainPort2.Post(new LaserRangeFinderResetUpdate(reset.Body));
}
/// <summary>
/// Handle the <typeparamref name="LaserRangeFinderResetUpdate"/> request.
/// </summary>
/// <remarks>Stops the robot.</remarks>
/// <param name="update">request</param>
void LaserRangeFinderResetUpdateHandler2(LaserRangeFind erResetUpdate update)
{
update.ResponsePort.Post(DefaultUpdateResponseType .Instance);

}

//ROBOT 3:
/// <summary>
/// Handles Replace notifications from the Laser partner
/// </summary>
/// <remarks>Posts a <typeparamref name="LaserRangeFinderUpdate"/> to itself.</remarks>
/// <param name="replace">notification</param>
/// <returns>task enumerator</returns>
IEnumerator<ITask> LaserReplaceNotification3(sicklrf3.Replace replace)
{
// When this handler is called a couple of notifications may
// have piled up. We only want the most recent one.
sicklrf3.State laserData = GetMostRecentLaserNotification3(replace.Body);
LaserRangeFinderUpdate request = new LaserRangeFinderUpdate(laserData);
_mainPort3.Post(request);
yield return Arbiter.Choice(
request.ResponsePort,
delegate(DefaultUpdateResponseType response) { },
delegate(Fault fault) { }
);
// Skip messages that have been queued up in the meantime.
// The notification that are lingering are out of data by now.
GetMostRecentLaserNotification3(laserData);
// Reactivate the handler.
Activate(
Arbiter.ReceiveWithIterator<sicklrf3.Replace>(fals e, _lrfNotifyPort3, LaserReplaceNotification3)
);
}
/// <summary>
/// Handles the <typeparamref name="LaserRangeFinderUpdate"/> request.
/// </summary>
/// <param name="update">request</param>
void LaserRangeFinderUpdateHandler3(LaserRangeFinderUpd ate update)
{
sicklrf3.State laserData = update.Body;
_state.PreviousLaser3 = _state.MostRecentLaser3;
_state.MostRecentLaser3 = laserData.TimeStamp;
TimeSpan ts = _state.MostRecentLaser3 - _state.PreviousLaser3;
int interval = ts.Milliseconds;
if (_state.DrawMap)
SpawnIterator(laserData.AngularRange, laserData.DistanceMeasurements, UpdateMap3);
update.ResponsePort.Post(DefaultUpdateResponseType .Instance);
}
/// <summary>
/// Gets the most recent laser notification. Older notifications are dropped.
/// </summary>
/// <param name="laserData">last known laser data</param>
/// <returns>most recent laser data</returns>
private sicklrf3.State GetMostRecentLaserNotification3(sicklrf3.State laserData)
{
sicklrf3.Replace testReplace;
int count = _lrfNotifyPort3.P3.ItemCount - 1;
for (int i = 0; i < count; i++)
{
testReplace = _lrfNotifyPort3.Test<sicklrf3.Replace>();
if (testReplace.Body.TimeStamp > laserData.TimeStamp)
{
laserData = testReplace.Body;
}
}

if (count > 0)
{
LogInfo(string.Format("Dropped {0} laser readings (laser start)", count));
}
return laserData;
}
/// <summary>
/// Handles the reset notification of the Laser partner.
/// </summary>
/// <remarks>Posts a <typeparamref name="LaserRangeFinderResetUpdate"/> to itself.</remarks>
/// <param name="reset">notification</param>
void LaserResetNotification3(sicklrf3.Reset reset)
{
_mainPort3.Post(new LaserRangeFinderResetUpdate(reset.Body));
}
/// <summary>
/// Handle the <typeparamref name="LaserRangeFinderResetUpdate"/> request.
/// </summary>
/// <remarks>Stops the robot.</remarks>
/// <param name="update">request</param>
void LaserRangeFinderResetUpdateHandler3(LaserRangeFind erResetUpdate update)
{
update.ResponsePort.Post(DefaultUpdateResponseType .Instance);

}


#endregion

#region Drive helper method

/// <summary>
/// Enables the drive
/// </summary>
/// <returns>response port</returns>
PortSet<DefaultUpdateResponseType, Fault> EnableMotor()
{
return EnableMotor(true);
}

/// <summary>
/// Disables the drive
/// </summary>
/// <returns>repsonse port</returns>
PortSet<DefaultUpdateResponseType, Fault> DisableMotor()
{
return EnableMotor(false);
}

/// <summary>
/// Sets the drives enabled state.
/// </summary>
/// <param name="enable">new enables state</param>
/// <returns>response port</returns>
PortSet<DefaultUpdateResponseType, Fault> EnableMotor(bool enable)
{
dd.EnableDriveRequest request = new dd.EnableDriveRequest();
request.Enable = enable;
return _drivePort.EnableDrive(request);
}
#endregion

#region Mapping
// This region contains all of the mapping code

// This is the map object
private static Mapper _map = null;
// The Windows Form for displaying the map
MapForm _mapForm = null;
// A bitmap used for displaying the map
Bitmap _mapBitmap;

/// <summary>
/// Update the map when new laser data arrives
/// </summary>
/// <param name="AngularRange"></param>
/// <param name="DistanceMeasurements"></param>
/// <returns></returns>
IEnumerator<ITask> UpdateMap(int AngularRange, int[] DistanceMeasurements)
{
// Insurance in case the LRF is not behaving ...
if (DistanceMeasurements == null)
yield break;

// Create a new map if we don't have one
if (_map == null)
{
double startAngle = AngularRange / 2.0;
// AngularResolution is zero for the simulated LRF!
// This is a bug that I have reported and it should be
// fixed in V1.5. In the meantime, calculate the value.
double angleIncrement = (double)AngularRange / (double)DistanceMeasurements.Length;
// Create the mapper object
_map = new Mapper();
// Set the drawing mode
_map.mode = _state.DrawingMode;
// Set the parameters and allocate the necessary memory
_mapBitmap = _map.Init(DistanceMeasurements.Length, _state.MapMaxRange,
startAngle, angleIncrement,
_state.MapWidth, _state.MapHeight,
_state.MapResolution);
// Clear the map initially (could be moved to Alloc)
_map.Clear();
}

// Make sure that we have a Windows Form to display the map
if (_mapForm == null)
MakeForm();

//Pose Update
DriveUpdate();

//_map.DrawMap(_mapBitmap, -_state.X, _state.Y, (_state.Theta - 90),
// AngularRange, DistanceMeasurements);
//DisplayImage(_mapBitmap);

// Add new laser data to the existing map
_map.Add(-_state.X, _state.Y, (_state.Theta - 90),
DistanceMeasurements.Length, DistanceMeasurements);
// Display the new map by requesting a bitmap image and then
// passing it to the code inside the Windows Form
DisplayImage(_map.MapToImage());

}
IEnumerator<ITask> UpdateMap2(int AngularRange, int[] DistanceMeasurements)
{
// Insurance in case the LRF is not behaving ...
if (DistanceMeasurements == null)
yield break;

// Create a new map if we don't have one
if (_map == null)
{
double startAngle = AngularRange / 2.0;
// AngularResolution is zero for the simulated LRF!
// This is a bug that I have reported and it should be
// fixed in V1.5. In the meantime, calculate the value.
double angleIncrement = (double)AngularRange / (double)DistanceMeasurements.Length;
// Create the mapper object
_map = new Mapper();
// Set the drawing mode
_map.mode = _state.DrawingMode;
// Set the parameters and allocate the necessary memory
_mapBitmap = _map.Init(DistanceMeasurements.Length, _state.MapMaxRange,
startAngle, angleIncrement,
_state.MapWidth, _state.MapHeight,
_state.MapResolution);
// Clear the map initially (could be moved to Alloc)
_map.Clear();
}

// Make sure that we have a Windows Form to display the map
if (_mapForm == null)
MakeForm();

//Pose Update
DriveUpdate2();

// Add new laser data to the existing map
_map.Add(-_state.X2, _state.Y2, (_state.Theta2 - 90),
DistanceMeasurements.Length, DistanceMeasurements);
// Display the new map by requesting a bitmap image and then
// passing it to the code inside the Windows Form
DisplayImage(_map.MapToImage());
}
IEnumerator<ITask> UpdateMap3(int AngularRange, int[] DistanceMeasurements)
{
// Insurance in case the LRF is not behaving ...
if (DistanceMeasurements == null)
yield break;

// Create a new map if we don't have one
if (_map == null)
{
double startAngle = AngularRange / 2.0;
// AngularResolution is zero for the simulated LRF!
// This is a bug that I have reported and it should be
// fixed in V1.5. In the meantime, calculate the value.
double angleIncrement = (double)AngularRange / (double)DistanceMeasurements.Length;
// Create the mapper object
_map = new Mapper();
// Set the drawing mode
_map.mode = _state.DrawingMode;
// Set the parameters and allocate the necessary memory
_mapBitmap = _map.Init(DistanceMeasurements.Length, _state.MapMaxRange,
startAngle, angleIncrement,
_state.MapWidth, _state.MapHeight,
_state.MapResolution);
// Clear the map initially (could be moved to Alloc)
_map.Clear();
}

// Make sure that we have a Windows Form to display the map
if (_mapForm == null)
MakeForm();

//Pose Update
DriveUpdate3();

// Add new laser data to the existing map
_map.Add(-_state.X3, _state.Y3, (_state.Theta3 - 90),
DistanceMeasurements.Length, DistanceMeasurements);
// Display the new map by requesting a bitmap image and then
// passing it to the code inside the Windows Form
DisplayImage(_map.MapToImage());
}

// Create and display the Windows Form containing the map
bool MakeForm()
{
Fault fault = null;

RunForm runForm = new RunForm(CreateMapForm);
//Console.WriteLine("Trying To Create Form");
WinFormsServicePort.Post(runForm);

Arbiter.Choice(
runForm.pResult,
delegate(SuccessResult success) { },
delegate(Exception e)
{
fault = Fault.FromException(e);
}
);

if (fault != null)
{
LogError(null, "Failed to create Map window", fault);
return false;
}
else
return true;
}

// The actual form creation needs to be called by RunForm
Form CreateMapForm()
{
_mapForm = new MapForm();
return _mapForm;
}

// Display a bitmap in the map window
bool DisplayImage(Bitmap bmp)
{
Fault fault = null;

FormInvoke setImage = new FormInvoke(
delegate()
{
if (bmp != null)
_mapForm.MapImage = bmp;
}
);

WinFormsServicePort.Post(setImage);

Arbiter.Choice(
setImage.ResultPort,
delegate(EmptyValue success) { },
delegate(Exception e)
{
fault = Fault.FromException(e);
}
);

if (fault != null)
{
LogError(null, "Unable to set camera image on form", fault);
return false;
}
else
{
// LogInfo("New camera frame");
return true;
}
}

#endregion

#endregion

//Connecting to Robot Entities: Differential Drives and Sensors
#region Robot Bindings and Call for Sensor Connections
//Insertion of Pioneers as Differential Drives
void InsertEntityNotificationHandler(simengine.InsertSi mulationEntity ins)
{
if (ins.Body.State.Name == "p3dx1")
{
_robot = (DifferentialDriveEntity)ins.Body;

LogInfo(LogGroups.Console, _robot.State.Name + " was connected!");
}
}
void DeleteEntityNotificationHandler(simengine.DeleteSi mulationEntity del)
{
if (del.Body.State.Name == "p3dx1")
{
_robot = null;
}
}
void InsertEntityNotificationHandler2(simengine.InsertS imulationEntity ins)
{
if (ins.Body.State.Name == "p3dx2")
{
_robot2 = (DifferentialDriveEntity)ins.Body;

LogInfo(LogGroups.Console, _robot2.State.Name + " was connected!");
}
}
void DeleteEntityNotificationHandler2(simengine.DeleteS imulationEntity del)
{
if (del.Body.State.Name == "p3dx2")
{
_robot2 = null;
}
}
void InsertEntityNotificationHandler3(simengine.InsertS imulationEntity ins)
{
if (ins.Body.State.Name == "p3dx3")
{
_robot3 = (DifferentialDriveEntity)ins.Body;
LogInfo(LogGroups.Console, _robot3.State.Name + " was connected! ");
SpawnIterator(Connections);
}
}
//Call for Connections() is included here
void DeleteEntityNotificationHandler3(simengine.DeleteS imulationEntity del)
{
if (del.Body.State.Name == "p3dx3")
{
_robot3 = null;
}
}
//Call for Perceptions() is included here
public IEnumerator<ITask> Connections()
{
//Sequential Processing For Bindings
///Robot 1
yield return Arbiter.ExecuteToCompletion(
TaskQueue,
new IterativeTask(ConnectRobot));
///Robot 2
yield return Arbiter.ExecuteToCompletion(
TaskQueue,
new IterativeTask(ConnectRobot2));
///Robot 3
yield return Arbiter.ExecuteToCompletion(
TaskQueue,
new IterativeTask(ConnectRobot3));
Console.WriteLine("ROBOTS BINDS DONE !");
Console.WriteLine("Calling For Sensors");
/// Bumper 1
yield return Arbiter.ExecuteToCompletion(
TaskQueue,
new IterativeTask(ConnectBumperSensor));
/// Bumper 2
yield return Arbiter.ExecuteToCompletion(
TaskQueue,
new IterativeTask(ConnectBumperSensor2));
/// Bumper 3
yield return Arbiter.ExecuteToCompletion(
TaskQueue,
new IterativeTask(ConnectBumperSensor3));
/// LRF 1
yield return Arbiter.ExecuteToCompletion(
TaskQueue,
new IterativeTask(ConnectLRFSensor));
/// LRF 2
yield return Arbiter.ExecuteToCompletion(
TaskQueue,
new IterativeTask(ConnectLRFSensor2));
/// LRF 3
yield return Arbiter.ExecuteToCompletion(
TaskQueue,
new IterativeTask(ConnectLRFSensor3));
Console.WriteLine("SENSORS BINDS DONE !");
Spawn(Perceptions);
yield break;
}
//ITasks for connecting each entity
///<Robots>
public IEnumerator<ITask> ConnectRobot()
{
string targetname = "p3dx1";
string contract = "http://schemas.microsoft.com/robotics/simulation/services/2006/05/simulateddifferentialdrive.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<CreateResponse, Fault> createResultPort;
createResultPort = base.CreateService(sInfo);
yield return createResultPort.Choice();
CreateResponse success = createResultPort;
if (success != null)
{
Console.ResetColor();
Console.ForegroundColor = ConsoleColor.Red;
Console.WriteLine("ROBOT " + targetname + " BIND DONE !");
Console.ResetColor();
_drivePort.Subscribe(_driveNotifyPort);
}
yield break;
}
public IEnumerator<ITask> ConnectRobot2()
{
string targetname = "p3dx2";
string contract = "http://schemas.microsoft.com/robotics/simulation/services/2006/05/simulateddifferentialdrive.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<CreateResponse, Fault> createResultPort;
createResultPort = base.CreateService(sInfo);
yield return createResultPort.Choice();
CreateResponse success = createResultPort;
if (success != null)
{
Console.ResetColor();
Console.ForegroundColor = ConsoleColor.Green;
Console.WriteLine("ROBOT " + targetname + " BIND DONE !");
Console.ResetColor();
_drivePort2.Subscribe(_driveNotifyPort2);
}
yield break;
}
public IEnumerator<ITask> ConnectRobot3()
{
string targetname = "p3dx3";
string contract = "http://schemas.microsoft.com/robotics/simulation/services/2006/05/simulateddifferentialdrive.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<CreateResponse, Fault> createResultPort;
createResultPort = base.CreateService(sInfo);
yield return createResultPort.Choice();
CreateResponse success = createResultPort;
if (success != null)
{
Console.ResetColor();
Console.ForegroundColor = ConsoleColor.Blue;
Console.WriteLine("ROBOT " + targetname + " BIND DONE !");
Console.ResetColor();
_drivePort3.Subscribe(_driveNotifyPort3);
}
yield break;
}
///<Bumpers>
public IEnumerator<ITask> ConnectBumperSensor()
{
string targetname = "p3dx1_bumper";
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<CreateResponse, Fault> createResultPort;
createResultPort = base.CreateService(sInfo);
yield return createResultPort.Choice();

CreateResponse success = createResultPort;
if (success != null)
{
Console.ResetColor();
Console.ForegroundColor = ConsoleColor.Red;
Console.WriteLine("BUMPER " + targetname + " BIND DONE !");
Console.ResetColor();
_bumperPort = ServiceForwarder<bumper.ContactSensorArrayOperatio ns>(success.Service);
_bumperPort.Subscribe(_bumperNotifyPor

Last edited by ChuyCepeda; February 26th, 2010 at 06:37 PM.
Reply With Quote
Reply


Thread Tools
Display Modes

Posting Rules
You may not post new threads
You may not post replies
You may not post attachments
You may not edit your posts

BB code is On
Smilies are On
[IMG] code is Off
HTML code is Off
Trackbacks are Off
Pingbacks are On
Refbacks are Off

Similar Threads
Thread Thread Starter Forum Replies Last Post
Error-Object does not support doesn't support this bootsy Classic ASP Basics 1 May 25th, 2008 07:14 PM
Does Hibernate 3, support to multiple database ish Hibernate 0 October 25th, 2007 10:16 AM
Trigger support for multiple Inserts kim3er SQL Server 2000 1 November 6th, 2004 01:51 AM
Importance of Robot file eapsokha HTML Code Clinic 1 August 30th, 2004 05:14 AM
How do I get support - can someone help me? andrewba Forum and Wrox.com Feedback 6 August 16th, 2004 12:25 PM



All times are GMT -4. The time now is 08:45 AM.


Powered by vBulletin® Version 3.7.0
Copyright ©2000 - 2014, Jelsoft Enterprises Ltd.
© 2013 John Wiley & Sons, Inc.