IEnumerator<ITask> OnSonarReplaceHandler(pxsonar.Replace replace)
{
if (_driveControl != null)
{
WinFormsServicePort.FormInvoke(
delegate()
{
// LogInfo("Llamando a ReplaceSonarData");
_driveControl.ReplaceSonarData(replace.Body);
}
);
}
// LogObject(replace.Body);
yield break;
}
public IEnumerator<ITask> ReplaceHandler(pxsonar.Replace replace)
{
_state = replace.Body;
if (replace.ResponsePort != null)
replace.ResponsePort.Post(dssp.DefaultReplaceResponseType.Instance);
// issue notification
_subMgrPort.Post(new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest));
yield break;
}
// Raul - Move for Sonar data
/// <summary>
/// Implements the "Moving" meta state.
/// </summary>
/// <param name="sonarData">most recently sensed sonar range data</param>
/// <param name="distance">closest obstacle in corridor ahead</param>
private void Move(pxsonar.SonarState sonarData, double distance)
{
switch (_state.LogicalState)
{
case LogicalState.AdjustHeading:
AdjustHeading();
break;
case LogicalState.FreeForwards:
AdjustVelocity(sonarData, distance);
break;
default:
LogInfo("ExplorerSimSonar.Move() called in illegal state");
break;
}
}
// Raul - Map with Sonar data
/// <summary>
/// Implements the "Mapping" meta state.
/// </summary>
/// <param name="sonarData">most recently sensed sonar range data</param>
/// <param name="distance">closest obstacle in corridor ahead</param>
private void Map(pxsonar.SonarState sonarData, double distance)
{
switch (_state.LogicalState)
{
case LogicalState.RandomTurn:
LogInfo("Random Turn"); // TT
RandomTurn();
break;
case LogicalState.MapSurroundings:
_state.Mapped = true;
LogInfo("Turning 180 deg to map");
Turn(180);
_state.LogicalState = LogicalState.MapSouth;
_state.Countdown = 15;
break;
case LogicalState.MapSouth:
LogInfo("Mapping the View South");
_state.SouthSonar = sonarData;
Turn(180);
_state.LogicalState = LogicalState.MapNorth;
_state.Countdown = 15;
break;
case LogicalState.MapNorth:
LogInfo("Mapping the View North");
_state.NewHeading = FindBestComposite(_state.SouthSonar, sonarData);
LogInfo("Map suggest turn: " + _state.NewHeading);
_state.South = null;
_state.LogicalState = LogicalState.AdjustHeading;
break;
default:
LogInfo("ExplorerSimSonar.Map() called in illegal state");
break;
}
}
// Raul - August 2007. Draw SONAR ring readings
private Bitmap DrawSonarRing(pxsonar.SonarState stateType)
{
// Graphic scale factor (milimeters to pixels)
// This is appropiate for 8 sonar transducers (just one sonar array)!
const int ScaleFactor = 40;
const int bottomMargin = 10;
float half_cone_rads = (float)((options.SonarTransducerAngularRange / 2) * Math.PI) / 180;
Color col;
Bitmap bmp = new Bitmap(
(options.SonarRange * 2)/ScaleFactor, // Width
(options.SonarRange/ScaleFactor) + bottomMargin ); // Height
Graphics g = Graphics.FromImage(bmp);
g.Clear(Color.White);
float x1 = bmp.Width / 2;
float y1 = bmp.Height - bottomMargin;
if (stateType.DistanceMeasurements.Length != options.SonarRadians.Length)
{
// This code needs to be improved. Graphical representation depends on Sonar Array arrangement.
}
else
{
lblSonarS0.Text = String.Format("S0: {0}", (int)stateType.DistanceMeasurements[0]);
lblSonarS1.Text = String.Format("S1: {0}", (int)stateType.DistanceMeasurements[1]);
lblSonarS2.Text = String.Format("S2: {0}", (int)stateType.DistanceMeasurements[2]);
lblSonarS3.Text = String.Format("S3: {0}", (int)stateType.DistanceMeasurements[3]);
lblSonarS4.Text = String.Format("S4: {0}", (int)stateType.DistanceMeasurements[4]);
lblSonarS5.Text = String.Format("S5: {0}", (int)stateType.DistanceMeasurements[5]);
lblSonarS6.Text = String.Format("S6: {0}", (int)stateType.DistanceMeasurements[6]);
lblSonarS7.Text = String.Format("S7: {0}", (int)stateType.DistanceMeasurements[7]);
for (int i = 0; i < stateType.DistanceMeasurements.Length; i++)
{
int range = (int)stateType.DistanceMeasurements[i];
if (range > 0 && range <= options.SonarRange)
{
float scaled_range = (float)range / ScaleFactor;
col = LinearColor(Color.DarkBlue, Color.LightGray, 0, (int)options.SonarRange, range);
Pen myPen = new Pen(col);
// X = scaled_range * Sin( transducer_orientation )
float x2 = bmp.Width - x1 - scaled_range * (float)Math.Sin(options.SonarRadians[i]);
// Y = scaled_range * Cos (transduCer_orientation)
float y2 = y1 - (scaled_range * (float)Math.Cos(options.SonarRadians[i]));
float m3 = scaled_range * (float)Math.Cos(half_cone_rads); // Side of the Cone = range * Cos(8 degrees)
float y3 = y1 - (m3 * (float)Math.Cos(options.SonarRadians[i] - half_cone_rads));
float x3 = x1 - (m3 * (float)Math.Sin(options.SonarRadians[i] - half_cone_rads));
float y4 = y1 - (m3 * (float)Math.Cos(options.SonarRadians[i] + half_cone_rads));
float x4 = x1 - (m3 * (float)Math.Sin(options.SonarRadians[i] + half_cone_rads));
// Raul - Here I draw all the lines
// You can comment out the lines you don't want to have in the Control Panel representation
// Draw the 2D cone bisector
g.DrawLine(new Pen(Color.OrangeRed),
x1, // I consider all rays coming out from a single point.
y1, // and from the bottom of the image.
x2,
y2);
// Draw right frontal range
g.DrawLine(myPen, x2, y2, x3, y3);
// Draw left frontal range
g.DrawLine(myPen, x2, y2, x4, y4);
// Draw right side of the cone
g.DrawLine(myPen, x1, y1, x3, y3);
// Draw left side of the cone
g.DrawLine(myPen, x1, y1, x4, y4);
}
}
}
// Return the image
return bmp;
}
// Raul - UpdateLogicalState for Sonar
/// <summary>
/// Transitions to the most appropriate state.
/// </summary>
/// <param name="sonarData">most recently sensed sonar range data</param>
/// <param name="distance">closest obstacle in corridor ahead</param>
private void UpdateLogicalState(pxsonar.SonarState sonarData, double distance)
{
if (_state.Countdown > 0)
{
_state.Countdown--;
}
else if (_state.IsUnknown)
{
StartMapping(sonarData, distance);
}
else if (_state.IsMoving)
{
Move(sonarData, distance);
}
else if (_state.IsMapping)
{
Map(sonarData, distance);
}
}
// Raul - FindBestFrom for Sonar data
/// <summary>
/// Finds the best heading in a 180 degree sonar scan
/// </summary>
/// <param name="sonarData">sonar scan</param>
/// <param name="dx">horizontal offset</param>
/// <param name="dy">vertical offset</param>
/// <param name="width">width of corridor that must be free</param>
/// <returns>best heading in degrees</returns>
private int FindBestFrom(pxsonar.SonarState sonarData, double dx, double dy, double width)
{
int count = sonarData.DistanceMeasurements.Length;
// double span = Math.PI * sonarData.AngularRange / 180.0;
int result;
List<RangeData> ranges = new List<RangeData>();
for (int i = 0; i < count; i++)
{
double range = sonarData.DistanceMeasurements[i];
double angle; // Raul - angle = span * i / count - span / 2.0;
if (i < _state.SonarTransducers)
{
angle = _state.SonarRadians[i];
}
else
{
angle = -_state.SonarRadians[i-_state.SonarTransducers];
}
double x = range * Math.Sin(angle) - dx;
double y = range * Math.Cos(angle) - dy;
angle = Math.Atan2(-x, y);
range = (int)Math.Sqrt(x * x + y * y);
ranges.Add(new RangeData(range, angle));
}
ranges.Sort(RangeData.ByDistance);
for (int i = 0; i < ranges.Count; i++)
{
RangeData curr = ranges[i];
double delta = Math.Atan2(width, curr.Distance);
double low = curr.Heading - delta;
double high = curr.Heading + delta;
for (int j = i + 1; j < ranges.Count; j++)
{
if (ranges[j].Heading > low &&
ranges[j].Heading < high)
{
ranges.RemoveAt(j);
j--;
}
}
}
ranges.Reverse();
double bestDistance = ranges[0].Distance;
double bestHeading = ranges[0].Heading;
Random rand = new Random();
for (int i = 0; i < ranges.Count; i++)
{
if (ranges[i].Distance < bestDistance)
{
break;
}
if (rand.Next(i + 1) == 0)
{
bestHeading = ranges[i].Heading;
}
}
// TT - Log the value
result = -(int)Math.Round(180 * bestHeading / Math.PI);
LogInfo("Find Best Heading: " + result);
return result;
}
public virtual IEnumerator<ITask> SubscribeHandler(pxsonar.Subscribe subscribe)
{
SubscribeRequestType request = subscribe.Body;
LogInfo("Subscribe request from: " + request.Subscriber);
// Use the Subscription Manager to insert the new subscriber
yield return Arbiter.Choice(
SubscribeHelper(_submgrPort, request, subscribe.ResponsePort),
delegate(SuccessResult success)
{
base.SendNotification<Replace>(_submgrPort, request.Subscriber, _state);
},
delegate(Exception e)
{
LogError(null, "Subscribe failed", e);
}
);
yield break;
}
/// <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 pxsonar.SonarState GetMostRecentSonarNotification(pxsonar.SonarState sonarData)
{
pxsonar.Replace testReplace;
// _laserNotify is a PortSet<>, P3 represents IPort<sicklrf.Replace> that
// the portset contains
int count = _sonarNotify.P3.ItemCount - 1;
for (int i = 0; i < count; i++)
{
testReplace = _sonarNotify.Test<pxsonar.Replace>();
if (testReplace.Body.TimeStamp > sonarData.TimeStamp)
{
sonarData = testReplace.Body;
}
}
if (count > 0)
{
LogInfo(string.Format("Dropped {0} sonar readings (sonar start)", count));
}
return sonarData;
}
// Raul - Find best composite for Sonar Data.
/// <summary>
/// Finds the best free corridor (maximum free space ahead) in a 360 degree scan.
/// </summary>
/// <param name="south">the backward half of the scan</param>
/// <param name="north">the forward half of the scan</param>
/// <returns>best heading in degrees</returns>
private int FindBestComposite(pxsonar.SonarState south, pxsonar.SonarState north)
{
pxsonar.SonarState composite = new pxsonar.SonarState();
// Raul - I am assuming just one 8 transducer frontal sonar ring.
// Raul - That means just 8 measuments per 180 degrees.
// Raul - Therefore a 360 scan has SonarTranducers * 2 measurements.
composite.DistanceMeasurements = new double[_state.SonarTransducers*2];
for (int i = 0; i < _state.SonarTransducers*2; i++)
{
if (i < _state.SonarTransducers)
{
composite.DistanceMeasurements[i] = south.DistanceMeasurements[i];
}
else
{
composite.DistanceMeasurements[i] = north.DistanceMeasurements[i-_state.SonarTransducers];
}
}
composite.AngularResolution = 22.5;
composite.AngularRange = 360;
// composite.Units = north.Units;
return FindBestFrom(composite, 0, 0, CorridorWidthMoving);
}
// Raul - Start mapping for Sonar data
/// <summary>
/// Transitions to "Mapping" meta state or "AdjustHeading" state depending on
/// environment.
/// </summary>
/// <param name="sonarData">most recently sensed sonar range data</param>
/// <param name="distance">closest obstacle in corridor ahead</param>
private void StartMapping(pxsonar.SonarState sonarData, double distance)
{
StopMoving();
if (distance < ObstacleDistance)
{
if (_state.Mapped)
{
// We have been mapping before but do not seem to
// have found anything.
_state.LogicalState = LogicalState.RandomTurn;
}
else
{
_state.LogicalState = LogicalState.MapSurroundings;
}
}
else
{
double step = Math.Min(ObstacleDistance, distance - CorridorWidthMapping);
// find the best angle from step mm in front of
// our current position
_state.NewHeading = FindBestFrom(sonarData, 0, step, CorridorWidthMapping);
LogInfo("Step: " + step + " Turn: " + _state.NewHeading);
Translate((int)step);
_state.LogicalState = LogicalState.AdjustHeading;
_state.Countdown = (int)step / 50 + Math.Abs(_state.NewHeading / 10);
}
}
// Raul - Adjust velocity for sonarData
/// <summary>
/// Adjusts the velocity based on environment.
/// </summary>
/// <param name="sonarData">most recently sensed sonar range data</param>
/// <param name="distance">closest obstacle in corridor ahead</param>
private void AdjustVelocity(pxsonar.SonarState sonarData, double distance)
{
_state.Mapped = false;
// Why does AdjustVelocity call Turn() ???
// This does not seem to make any sense, and the robot gets
// stuck in oscillations to the left and right
double test = FindBestFrom(sonarData, 0, _state.Velocity / 10, CorridorWidthMoving);
if (distance > FreeDistance)
{
MoveForward(MaximumForwardVelocity);
//if (Math.Abs(test) < 10)
if (Math.Abs(test) < 10)
{
Turn((int)test / 2);
// TT Jun-2007 - Changed to slow down
_state.Countdown = (int)Math.Abs((test / 2) / 5);
}
}
else if (distance > AwareOfObstacleDistance)
{
MoveForward(MaximumForwardVelocity / 2);
//if (Math.Abs(test) < 45)
if (Math.Abs(test) < 45)
{
Turn((int)test / 2);
// TT Jun-2007 - Changed
_state.Countdown = (int)Math.Abs((test / 2) / 5);
}
}
else
{
MoveForward(MaximumForwardVelocity / 4);
if (Math.Abs(test) < 60)
{
Turn((int)test);
// TT Jun-2007 - Changed
//_state.Countdown = Math.Abs(test / 10);
_state.Countdown = 5;
}
}
}
public void ReplaceSonarData(pxsonar.SonarState stateType)
{
if (stateType.TimeStamp < _lastSonar)
{
return;
}
_lastSonar = stateType.TimeStamp;
TimeSpan delay = DateTime.Now - stateType.TimeStamp;
lblSonarDelay.Text = delay.ToString();
// Raul - Sept 2007
if (options.DisplaySonarMap)
{
picSonar.Image = DrawSonarMap(stateType);
}
else
{
picSonar.Image = DrawSonarRing(stateType);
}
if (btnConnectSonar.Enabled)
{
btnConnectSonar.Enabled = false;
}
if (!btnDisconnectSonar.Enabled)
{
btnDisconnectSonar.Enabled = true;
}
}
public virtual IEnumerator<ITask> GetHandler(pxsonar.Get get)
{
get.ResponsePort.Post(_state);
yield break;
}
// Raul - Finds closest obstacle in a corridor for sonar data
/// <summary>
/// Finds closest obstacle in a corridor.
/// </summary>
/// <param name="sonarData">sonar scan</param>
/// <param name="width">corridor width</param>
/// <param name="fov">field of view in degrees</param>
/// <returns>distance to the closest obstacle</returns>
private double FindNearestObstacleInCorridor(pxsonar.SonarState sonarData, int width, int fov)
{
int index;
double best = 8192;
// TT - There is a timing issue during startup whereby the
// LRF actually does not supply any data! Just ignore it.
if (sonarData == null || sonarData.DistanceMeasurements == null)
return 0;
int count = sonarData.DistanceMeasurements.Length;
double rangeLow = -sonarData.AngularRange / 2.0;
double rangeHigh = sonarData.AngularRange / 2.0;
double span = sonarData.AngularRange;
for (index = 0; index < count; index++)
{
double angle = rangeLow + (span * index) / count;
if (Math.Abs(angle) < fov)
{
angle = angle * Math.PI / 180;
double range = sonarData.DistanceMeasurements[index];
int x = (int)(range * Math.Sin(angle));
int y = (int)(range * Math.Cos(angle));
if (Math.Abs(x) < width)
{
if (range < best)
{
best = range;
}
}
}
}
// TT - Log the value
//LogInfo("Nearest Obstacle: " + best);
return best;
}
public virtual IEnumerator<ITask> ReplaceHandler(pxsonar.Replace replace)
{
_state = replace.Body;
replace.ResponsePort.Post(DefaultReplaceResponseType.Instance);
yield break;
}
// Raul - Sonar replace notification
/// <summary>
/// Handles Replace notifications from the Sonar partner
/// </summary>
/// <remarks>Posts a <typeparamref name="LaserRangeFinderUpdate"/> to itself.</remarks>
/// <param name="replace">notification</param>
/// <returns>task enumerator</returns>
IEnumerator<ITask> SonarReplaceNotification(pxsonar.Replace replace)
{
// When this handler is called a couple of notifications may
// have piled up. We only want the most recent one.
pxsonar.SonarState sonarData = replace.Body;
SonarUpdate request = new SonarUpdate(sonarData);
_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.
GetMostRecentSonarNotification(sonarData);
// Reactivate the handler.
Activate(
Arbiter.ReceiveWithIterator<pxsonar.Replace>(false, _sonarNotify, SonarReplaceNotification)
);
}
public virtual IEnumerator<ITask> ReliableSubscribeHandler(pxsonar.ReliableSubscribe subscribe)
{
yield return Arbiter.Choice(
SubscribeHelper(_submgrPort, subscribe.Body, subscribe.ResponsePort),
delegate(SuccessResult success)
{
_submgrPort.Post(new submgr.Submit(
subscribe.Body.Subscriber, dssp.DsspActions.ReplaceRequest, _state, null));
},
delegate(Exception ex) { LogError(ex); }
); ;
}
/*
DateTime lastFace = new DateTime(1999, 1, 1);
DateTime lastLeftHand = new DateTime(1999, 1, 1);
DateTime lastRightHand = new DateTime(1999, 1, 1);
DateTime lastObject = new DateTime(1999, 1, 1);
public void DrawMotionImage(Bitmap image)
{
this.MotionImage = image;
TimeSpan span = DateTime.Now - lastFace;
if (span.Seconds > 1)
{
picVisionFace.Visible = false;
}
span = DateTime.Now - lastLeftHand;
if (span.Seconds > 1)
{
picLeftHand.Visible = false;
}
span = DateTime.Now - lastRightHand;
if (span.Seconds > 1)
{
picRightHand.Visible = false;
}
span = DateTime.Now - lastObject;
if (span.Seconds > 1)
{
picVisionObject.Visible = false;
}
}
public void DrawFace(vision.FaceResult face)
{
if (face.HeadFound)
{
picVisionFace.Visible = true;
lastFace = DateTime.Now;
}
}
public void DrawHand(vision.HandGestureResult hand)
{
if (hand.LeftHandGestureFound)
{
if (hand.LeftHandGestureFound)
{
picLeftHand.Visible = true;
lastLeftHand = DateTime.Now;
}
if ( hand.RightHandGestureFound)
{
picRightHand.Visible = true;
lastRightHand = DateTime.Now;
}
}
}
public void DrawObject(vision.ObjectResult vobject)
{
if (vobject.ObjectFound)
{
picVisionObject.Visible = true;
lastObject = DateTime.Now;
}
}
*/
#endregion
// Raul - Aug 2007 - Draw an occupancy grid style map using the SONAR data
private Bitmap DrawSonarMap(pxsonar.SonarState stateType)
{
// Graphic scale factor (milimeters to pixels)
const int ScaleFactor = 40;
const int bottomMargin = 10;
float scaled_maxRange = options.SonarRange / ScaleFactor;
float half_cone = options.SonarTransducerAngularRange / 2;
// Set the dimension of the local map image
Bitmap bmp = new Bitmap(
(options.SonarRange * 2) / ScaleFactor, // Width
(options.SonarRange / ScaleFactor) ); // Height
Graphics g = Graphics.FromImage(bmp);
g.Clear(Color.LightGray);
// This is only valid for a single 8 transducer array so far
// !!!!
if (stateType.DistanceMeasurements.Length > 8)
{
return bmp;
}
// Draw the free space. Same as noted by Trevor in DrawLRFMap.
// "slivers" that are not filled in between the cones.
double angle;
double cone_angles;
double scaled_range;
double rx, ry;
int ix, iy;
float x3=0, y3=0, x4=0, y4=0;
Pen fatPen = new Pen(Color.White, 2.0f);
// For each Sonar transducer
for (int i = 0; i < stateType.DistanceMeasurements.Length; i++)
{
scaled_range = stateType.DistanceMeasurements[i] / ScaleFactor;
angle = options.SonarRadians[i];
// Draw current cone
for (float j = -half_cone; j <= half_cone; j+=1.0f)
{
cone_angles = options.SonarRadians[i] + j * Math.PI / 180;
rx = scaled_range * Math.Cos(cone_angles);
ry = scaled_range * Math.Sin(cone_angles);
ix = bmp.Width - bmp.Width / 2 - (int)(ry * scaled_maxRange / scaled_maxRange);
iy = bmp.Height - bottomMargin - (int)(rx * scaled_maxRange / scaled_maxRange);
// get the borders of this cone
if (j == -half_cone) // center of the cone
{
x3 = ix;
y3 = iy;
}
if (j == half_cone)
{
x4 = ix;
y4 = iy;
}
g.DrawLine(fatPen,
bmp.Width / 2,
bmp.Height - bottomMargin,
ix,
iy);
}
// Draw obstacles at the points of Sonar hits
if (scaled_range < scaled_maxRange)
{
g.DrawLine(Pens.Black, x3, y3, x4, y4);
}
}
// Return the image
return bmp;
}
