public IEnumerator <ITask> GetHandler(diffdrive.Get get)
{
UpdateStateFromSimulation();
get.ResponsePort.Post(_state);
yield break;
}
public IEnumerator <ITask> GetHandler(drive.Get get)
{
get.ResponsePort.Post(_state);
yield break;
}
/// <summary>
/// Update the map when new laser data arrives
/// </summary>
/// <param name="AngularRange"></param>
/// <param name="DistanceMeasurements"></param>
/// <returns></returns>
IEnumerator<ITask> UpdateMap(double AngularRange, double[] 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(_state.UseSonar, 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();
// Get the current pose from the Differential Drive
drive.Get GetRequest = new drive.Get();
_drivePort.Post(GetRequest);
// NOTE: Using yield return means that the code will not
// continue until the drive state has been returned
yield return Arbiter.Choice(
GetRequest.ResponsePort,
DriveStateUpdate,
delegate(Fault fault) { }
);
// Draw the map now using the new LRF data and the robot's pose
// Note that DrawMap used GDI primitives to draw into a bitmap
// This is one way to do it, but it is not very flexible
/*
_map.DrawMap(_mapBitmap, -_state.X, _state.Y, (_state.Theta - 90),
AngularRange, DistanceMeasurements);
DisplayImage(_mapBitmap);
*/
// Add new laser data to the existing map
int[] distances = new int[DistanceMeasurements.Length];
for (int i = 0; i < distances.Length; i++)
{
distances[i] = (int)DistanceMeasurements[i];
}
_map.Add(-_state.X, _state.Y, (_state.Theta - 90),
DistanceMeasurements.Length, distances);
// Display the new map by requesting a bitmap image and then
// passing it to the code inside the Windows Form
DisplayImage(_map.MapToImage());
}
