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()); }