/// <summary>
/// set Current Goal Bearing based on parameter bearingRelative
/// </summary>
/// <param name="bearingRelativeToRobot">Goal bearing relative to robot, degrees</param>
protected void setCurrentGoalBearingRelativeToRobot(double bearingRelativeToRobot)
{
double? goalBearing = _mapperVicinity.robotDirection.bearingRelative;
if (!goalBearing.HasValue || Math.Abs(goalBearing.Value - bearingRelativeToRobot) > 2.0d) // ignore differences less than 2 degrees
{
// update mapper with Direction data:
Direction newDir = new Direction()
{
TimeStamp = DateTime.Now.Ticks,
heading = _mapperVicinity.robotDirection.heading
};
newDir.bearingRelative = bearingRelativeToRobot; // will calculate absolute bearing
_mapperVicinity.robotDirection = newDir; // will also call mapperVicinity.computeMapPositions();
if ((DateTime.Now - lastPrintedBearing).TotalSeconds > 5.0d)
{
lastPrintedBearing = DateTime.Now;
Tracer.Trace("------------- goal bearing=" + _mapperVicinity.robotDirection.bearing + " (supplied bearingRelative=" + bearingRelativeToRobot + ")");
//Talker.Say(10, "" + Math.Round(bearingRelativeToRobot));
//Talker.Say(10, "" + Math.Round(_mapperVicinity.robotDirection.bearing));
}
setGuiCurrentDirection(newDir);
}
}
/// <summary>
/// fills cells that are along the path from the center to border
/// </summary>
/// <param name="angle">degrees, from vertical up, -180 to 180 or 0...360 (any angle will be brought to -180...180 range)</param>
protected CellPath shootRay(Direction dir)
{
CellPath cellPath = new CellPath();
bool hitObstacle = false;
double pathDistance = _mapper.robotState.robotLengthMeters / 2.0d;
MapCell mc = null;
double angle = (double)dir.bearing;
int startX = nW / 2;
int startY = nH / 2;
int robotWidthCells = (int)Math.Floor(_mapper.robotState.robotWidthMeters / MapperSettings.elementSizeMeters) + 1;
int halfWidth = (int)Math.Ceiling((robotWidthCells - 1) / 2.0d);
angle = Direction.to180(angle);
bool verticalUp = Math.Abs(angle) <= 1.0d;
bool verticalDown = angle >= 179.0d || angle <= -179.0d;
int y = startY;
int dy = angle > 90.0d || angle < -90.0d ? 1 : -1;
if (verticalUp || verticalDown)
{
int endY = verticalUp ? 0 : nH - 1;
while (!hitObstacle && y >= 0 && y < nH)
{
pathDistance = Math.Abs(y - startY) * MapperSettings.elementSizeMeters;
for (int xx = startX - halfWidth; !hitObstacle && xx <= startX + halfWidth; xx++)
{
hitObstacle = registerCell(cellPath, xx, y, pathDistance, out mc);
}
y += dy;
}
}
else
{
double angleR = (90.0d - angle) * Math.PI / 180.0d;
double factor = verticalUp ? 100000.0d : (verticalDown ? -100000.0d : Math.Tan(angleR));
int dx = angle > 0.0d ? 1 : -1;
bool spreadV = angle >= 45.0d && angle <= 135.0d || angle < -45.0d && angle > -135.0d;
for (int x = startX; !hitObstacle && x >= 0 && x < nW && y >= 0 && y < nH; x += dx)
{
double pathDistanceX = Math.Abs(x - startX) * MapperSettings.elementSizeMeters;
double pathDistanceY = Math.Abs(y - startY) * MapperSettings.elementSizeMeters;
pathDistance = Math.Sqrt(pathDistanceX * pathDistanceX + pathDistanceY * pathDistanceY);
if (pathDistance >= pathDistanceMax)
{
break;
}
int endY = Math.Max(0, Math.Min(startY - ((int)Math.Round((x - startX) * factor)), nH - 1));
while (!hitObstacle && y >= 0 && y < nH && (dy > 0 && y <= endY || dy < 0 && y >= endY))
{
if (spreadV)
{
hitObstacle = registerCell(cellPath, x, y, pathDistance, out mc);
}
else
{
for (int xx = Math.Max(0, x - halfWidth); !hitObstacle && xx <= Math.Min(x + halfWidth, nW - 1); xx++)
{
hitObstacle = registerCell(cellPath, xx, y, pathDistance, out mc);
}
}
y += dy;
}
if (spreadV)
{
for (int yy = Math.Max(0, endY - halfWidth); !hitObstacle && yy <= Math.Min(endY + halfWidth, nH - 1); yy++)
{
hitObstacle = registerCell(cellPath, x, yy, pathDistance, out mc);
}
}
else if(!hitObstacle)
{
hitObstacle = registerCell(cellPath, x, endY, pathDistance, out mc);
}
}
}
cellPath.lengthMeters = pathDistance;
cellPath.hitObstacle = hitObstacle;
return cellPath;
}
/// <summary>
/// creates a RoutePlan, based on mapper's robotDirection and cells on the geo plane (using _mapper.geoCellAt()); analyses obstacles (busy cells)
/// </summary>
/// <returns></returns>
public RoutePlan planRoute()
{
DateTime started = DateTime.Now;
RoutePlan plan = new RoutePlan();
if (_mapper.robotDirection.heading.HasValue)
{
lock (this)
{
try
{
cellPaths.Clear();
double sweepAngleHalf = sweepAngleNormal / 2.0d;
double goalBearingRelative = 0.0d; // straight in front is default
if (_mapper.robotDirection.bearing.HasValue)
{
goalBearingRelative = (double)_mapper.robotDirection.turnRelative;
}
if (Math.Abs(goalBearingRelative) > sweepAngleHalf)
{
// robot is pointing away from the goal, make him turn towards the goal first:
plan.bestHeading = Direction.to360(_mapper.robotDirection.course + goalBearingRelative);
plan.legMeters = null;
plan.closestObstacleMeters = _mapper.robotState.robotLengthMeters / 2.0d;
}
else
{
int nsteps = (int)Math.Round(sweepAngleHalf / raySweepDegrees);
for (int i = -nsteps; i < nsteps; i++)
{
double pathHeadingRelative = raySweepDegrees * i;
Direction dir = new Direction() { heading = _mapper.robotDirection.heading, bearingRelative = pathHeadingRelative }; // related to robot heading;
CellPath cellPath = shootRay(dir);
cellPath.firstHeadingRelative = pathHeadingRelative;
cellPaths.Add(cellPath);
}
// order (low to high) paths based on their length and deviation from the goal bearing - using pathRatingFunction():
CellPath bestPath = cellPaths.OrderBy(c => pathRatingFunction(c, goalBearingRelative, sweepAngleHalf)).Last();
bestPath.isBest = true;
plan.bestHeading = Direction.to360(_mapper.robotDirection.course + bestPath.firstHeadingRelative);
plan.legMeters = bestPath.lengthMeters;
plan.closestObstacleMeters = bestPath.lengthMeters - _mapper.robotState.robotLengthMeters;
}
}
catch (Exception exc)
{
Console.WriteLine("planRoute() - " + exc);
}
}
}
plan.timeSpentPlanning = DateTime.Now - started;
return plan;
}
/// <summary>
/// update map with the detected objects
/// </summary>
/// <param name="timestamp"></param>
private void updateMapperVicinity(long timestamp, SonarData p)
{
if (lastRangeReadingSetChanged && lastRangeReadingSet.sweepFrameReady && p.angles.Keys.Count == 26)
{
RangeReading rrFirst = p.getLatestReadingAt(p.angles.Keys.First());
RangeReading rrLast = p.getLatestReadingAt(p.angles.Keys.Last());
double sweepAngle = rrFirst.angleDegrees - rrLast.angleDegrees;
double forwardAngle = sweepAngle / 2.0d;
foreach (int angle in p.angles.Keys)
{
RangeReading rr = p.getLatestReadingAt(angle);
double rangeMeters = rr.rangeMeters + robotCornerDistanceMeters; // sensor is on the corner, adjust for that
double relBearing = rr.angleDegrees - forwardAngle;
GeoPosition pos1 = (GeoPosition)mapperVicinity.robotPosition.Clone();
pos1.translate(new Direction() { heading = mapperVicinity.robotDirection.heading, bearingRelative = relBearing }, new Distance(rangeMeters));
DetectedObstacle dobst1 = new DetectedObstacle(pos1) {
geoPosition = pos1,
firstSeen = timestamp,
lastSeen = timestamp,
color = Colors.Red,
detectorType = DetectorType.SONAR_SCANNING,
objectType = DetectedObjectType.Obstacle
};
mapperVicinity.Add(dobst1);
}
// make sure we have the latest heading info in the mapper, and map positions have been computed:
Direction dir = new Direction() { heading = lastDirData.heading, bearing = mapperVicinity.robotDirection.bearing, TimeStamp = timestamp };
mapperVicinity.robotDirection = dir; // will call mapperVicinity.computeMapPositions();
updateMapWindow();
}
mapperTraceLabel.Content = "" + mapperVicinity.detectedObjects.Count + " objects";
}
private void bearingTextBox_KeyUp(object sender, KeyEventArgs e)
{
double newBearing;
if (e.Key == Key.Enter)
{
MapperVicinity _mapperVicinity = mapperViewControl1.CurrentMapper;
if(_mapperVicinity != null && _mapperVicinity.robotDirection != null && _mapperVicinity.robotDirection.heading.HasValue && double.TryParse(bearingTextBox.Text, out newBearing))
{
newBearing = Direction.to360(newBearing);
Direction dir = new Direction() { heading = _mapperVicinity.robotDirection.heading, bearing = newBearing, TimeStamp = DateTime.Now.Ticks };
_mapperVicinity.robotDirection = dir; // will call mapperVicinity.computeMapPositions();
mapperViewControl1.RedrawMap();
}
}
}
public DetectedObstacle(Direction dir, Distance dist)
: base(dir, dist)
{
objectType = DetectedObjectType.Obstacle;
}
/// <summary>
/// update GUI with current direction
/// </summary>
/// <param name="dir"></param>
protected void setGuiCurrentDirection(Direction dir)
{
if (_mainWindow != null)
{
ccrwpf.Invoke invoke = new ccrwpf.Invoke(delegate()
{
_mainWindow.CurrentDirection = new DirectionData() { TimeStamp = dir.TimeStamp, heading = (double)dir.heading, bearing = _mapperVicinity.robotDirection.bearing };
}
);
wpfServicePort.Post(invoke);
Arbiter.Activate(TaskQueue,
invoke.ResponsePort.Choice(
s => { }, // delegate for success
ex => { } //Tracer.Error(ex) // delegate for failure
));
}
}
public DetectedObstacle(Direction dir, Distance dist)
: base(dir, dist)
{
SetObstacle();
}
private void bearingTextBox_KeyUp(object sender, KeyEventArgs e)
{
if (e.Key == Key.Enter)
{
double newBearing;
if (double.TryParse(bearingTextBox.Text, out newBearing))
{
newBearing = Direction.to360(newBearing);
Direction dir = new Direction() { heading = mapperVicinity.robotDirection.heading, bearing = newBearing, TimeStamp = DateTime.Now.Ticks };
mapperVicinity.robotDirection = dir; // will call mapperVicinity.computeMapPositions();
robotDirectionViewControl1.CurrentDirection = new DirectionData() { bearing = (double)dir.bearing, heading = (double)dir.heading, TimeStamp = dir.TimeStamp };
updateMapWindow();
}
}
}
private void bearingTextBox_KeyUp(object sender, KeyEventArgs e)
{
double newBearing;
if (e.Key == Key.Enter && _mapperVicinity != null && _mapperVicinity.robotDirection != null && _mapperVicinity.robotDirection.heading.HasValue && double.TryParse(bearingTextBox.Text, out newBearing))
{
newBearing %= 360.0d;
Direction dir = new Direction() { heading = _mapperVicinity.robotDirection.heading, bearing = newBearing, TimeStamp = DateTime.Now.Ticks };
_mapperVicinity.robotDirection = dir; // will call mapperVicinity.computeMapPositions();
robotDirectionViewControl1.CurrentDirection = new DirectionData() { bearing = dir.bearing, heading = (double)dir.heading, TimeStamp = dir.TimeStamp };
if (mapWindow != null)
{
mapWindow.mapperViewControl1.RedrawMap();
}
}
}
public DetectedHuman(Direction dir, Distance dist)
: base(dir, dist)
{
SetHuman();
}
// works only for small distances, within miles. For rare cases when we have both heading and bearing, but want to move in the "heading" direction.
public void translateToHeading(Direction dir, Distance by)
{
double range = by.Meters;
double angle = (double)dir.heading; // degrees
m_X += toDegreesX(new Distance(range * Math.Sin(angle * Math.PI / 180.0d)));
m_Y += toDegreesY(new Distance(range * Math.Cos(angle * Math.PI / 180.0d)));
}
// works only for small distances, within miles. For general case - when bearing is missing, or when we need to move towards bearing.
public void translate(Direction dir, Distance by)
{
double range = by.Meters;
double angle = (double)dir.heading; // degrees
if (dir.bearing.HasValue)
{
// if both heading and bearing are supplied, then heading represents robot direction, and bearing - absolute direction to the object.
// we are interested in translating in the "bearing" direction in this case, it is done to the objects relative to the robot.
angle = (double)dir.bearing;
}
m_X += toDegreesX(new Distance(range * Math.Sin(angle * Math.PI / 180.0d)));
m_Y += toDegreesY(new Distance(range * Math.Cos(angle * Math.PI / 180.0d)));
}
public Direction directionToWp(GeoPosition myPos, Direction myDir)
{
return new Direction() { heading = myDir.heading, bearing = myPos.bearing(this.geoPosition) };
}
public void setRobotPositionAndDirection(GeoPosition pos, Direction dir)
{
MapperVicinity mapperVicinity = CurrentMapper;
if (pos != null)
{
mapperVicinity.robotPosition = (GeoPosition)pos.Clone();
}
if (dir != null)
{
mapperVicinity.robotDirection = (Direction)dir.Clone();
}
// --------- debug ------------
/*
GeoPosition pos1 = (GeoPosition)pos.Clone();
pos1.translate(new Distance(1.0d), new Distance(1.0d)); // robot coordinates - right forward
DetectedObstacle dobst1 = new DetectedObstacle(pos1) { color = Colors.Red };
mapperVicinity.AddDetectedObject(dobst1);
GeoPosition pos2 = (GeoPosition)pos.Clone();
pos2.translate(new Distance(-1.0d), new Distance(1.0d)); // robot coordinates - left forward
DetectedObstacle dobst2 = new DetectedObstacle(pos2) { color = Colors.Yellow };
mapperVicinity.AddDetectedObject(dobst2);
mapperVicinity.computeMapPositions();
*/
// --------- end debug ------------
RedrawMap();
}
private void compassSlider_ValueChanged(object sender, RoutedPropertyChangedEventArgs<double> e)
{
if ((bool)compassCheckBox.IsChecked)
{
//Slider slider = (Slider)sender;
double newHeading = e.NewValue;
Direction dir = new Direction() { heading = newHeading, bearing = mapperVicinity.robotDirection.bearing, TimeStamp = DateTime.Now.Ticks };
mapperVicinity.robotDirection = dir; // will call mapperVicinity.computeMapPositions();
robotDirectionViewControl1.CurrentDirection = new DirectionData() { bearing = (double)dir.bearing, heading = (double)dir.heading, TimeStamp = dir.TimeStamp };
updateMapWindow();
}
}
public DetectedObjectBase(Direction dir, Distance dist)
: this()
{
relPosition = new RelPosition(dir, dist);
}
private void setViewControls(long timestamp)
{
// non-sonar data is displayed at our discretion, at time interval or real time:
viewControlsUpdateIntervalTicks = (long)updateDelaySlider.Value * 10000L;
if (timestamp - lastViewControlsUpdatedTimestamp > viewControlsUpdateIntervalTicks)
{
lastViewControlsUpdatedTimestamp = timestamp;
if (lastProxDataChanged)
{
lastProxDataChanged = false;
robotProximityViewControl1.CurrentValue = lastProxData;
}
if (lastAccDataChanged)
{
lastAccDataChanged = false;
lastAccData.computeVectors();
robotOrientationViewControl1.CurrentValue = lastAccData;
}
if (lastDirDataChanged)
{
lastDirDataChanged = false;
if (mapperVicinity.robotDirection.bearing.HasValue)
{
lastDirData.bearing = (double)mapperVicinity.robotDirection.bearing;
}
robotDirectionViewControl1.CurrentDirection = lastDirData;
if (mapWindow != null)
{
Direction dir = new Direction() { heading = lastDirData.heading, TimeStamp = timestamp };
mapWindow.mapperViewControl1.CurrentDirection = dir;
}
}
if (lastPsiDataChanged)
{
lastPsiDataChanged = false;
robotParkingSensorViewControl1.CurrentValue = lastPsiData;
}
}
}
private void TacticsFollowDirection()
{
Direction curDir = null;
//bool performedAvoidCollision = false;
if (_state.MovingState == MovingState.BumpedBackingUp)
{
Tracer.Trace("TacticsFollowDirection waiting MovingState " + _state.MovingState + " to finish");
return;
}
//else if (_mapperVicinity.robotState.doVicinityPlanning && FollowDirectionMaxVelocityMmSec > 0.01d)
//{
// if (_currentRoutePlan != null && _currentRoutePlan.isGoodPlan)
// {
// // AvoidCollision and EnterOpenSpace have precedence over
// // all other state transitions and are thus handled first.
// bool canMove = PerformAvoidCollision(_currentRoutePlan);
// if (!canMove)
// {
// StopMoving();
// _state.MovingState = MovingState.Unable;
// _state.Countdown = 0; // 0 = immediate response
// }
// performedAvoidCollision = true;
// // at this point constraints and allowable speeds and distances are known and stored in _state.collisionState
// // we must use them while executing the plan.
// // good place to analyze the plan against constraints and set MovingState.Unable
// _currentRoutePlan = critiquePlanChain(_currentRoutePlan);
// if (_currentRoutePlan != null && _currentRoutePlan.isGoodPlan)
// {
// double? distanceToGoalMeters = _mapperVicinity.robotDirection.distanceToGoalMeters; // can be null, but not negative
// if (distanceToGoalMeters.HasValue)
// {
// distanceToGoalMeters = Math.Min(distanceToGoalMeters.Value, _currentRoutePlan.legMeters.Value);
// }
// else
// {
// distanceToGoalMeters = _currentRoutePlan.legMeters.Value;
// }
// curDir = new Direction() {
// heading = _mapperVicinity.robotDirection.heading,
// bearing = _currentRoutePlan.bestHeading,
// distanceToGoalMeters = distanceToGoalMeters
// };
// }
// else
// {
// Tracer.Trace("Best Plan no good after critique");
// _state.MovingState = MovingState.Unable; // no good plan in the chain
// }
// }
// else
// {
// Tracer.Trace("Best Plan no good");
// _state.MovingState = MovingState.Unable; // no good plan in the chain
// //curDir = _mapperVicinity.robotDirection; // also in _currentGoalBearing
// }
//}
//else
{
// no planning needed - by the setting or while backing up. Ignore _currentRoutePlan
//bool canMove = PerformAvoidCollision(null);
//if (!canMove)
//{
// StopMoving();
// _state.MovingState = MovingState.Unable;
// _state.Countdown = 0; // 0 = immediate response
//}
//performedAvoidCollision = true;
// actual distance to target:
double? distanceToGoalMeters = _mapperVicinity.robotDirection.distanceToGoalMeters; // can be null, but not negative
curDir = new Direction() {
heading = _mapperVicinity.robotDirection.heading, // robot heading
bearing = _mapperVicinity.robotDirection.bearing, // bearing to target
distanceToGoalMeters = distanceToGoalMeters // actual distance to target
};
//distanceToGoalMeters = _state.collisionState.canMoveBackwardsDistanceMm / 1000.0d;
}
if (curDir != null && curDir.bearing.HasValue && curDir.bearingRelative.HasValue )
{
// Strategy asks us to turn or/and drive. We assume that collision avoidance has been taken care of.
// this is a marching order, no thinking.
//LogHistory(10, "Follow Direction: bearing=" + curDir.bearing + " heading=" + curDir.heading);
//Tracer.Trace("************** Follow Direction: bearing=" + curDir.bearing + " turnRelative=" + curDir.turnRelative + " heading=" + curDir.heading);
double turnRelative = curDir.turnRelative; // degrees -180...180
bool steepTurn = Math.Abs(turnRelative) > 40.0d;
if ((DateTime.Now - lastPrintedTurnRelative).TotalSeconds > 5.0d)
{
lastPrintedTurnRelative = DateTime.Now;
//Tracer.Trace("------------- turnRelative=" + turnRelative + " steepTurn=" + steepTurn);
//Talker.Say(10, "" + Math.Round(turnRelative) + (steepTurn ? " steep" : ""));
//Talker.Say(10, "" + Math.Round(_mapperVicinity.robotDirection.bearing));
}
long nowTicks = DateTime.Now.Ticks;
// call Angular Speed PID controller:
_state.followDirectionPidControllerAngularSpeed.Update(turnRelative, nowTicks);
double angularPidRate = _state.followDirectionPidControllerAngularSpeed.CalculateControl();
double? linearPidRate = null; // -100.0 ... +100.0
double distanceToCoverMm = 0.0d;
//Tracer.Trace("Follow Direction: steepTurn=" + steepTurn + " curDir.distanceToGoalMeters=" + curDir.distanceToGoalMeters + " FollowDirectionTargetDistanceToGoalMeters=" + FollowDirectionTargetDistanceToGoalMeters);
if (!steepTurn && curDir.distanceToGoalMeters.HasValue && FollowDirectionTargetDistanceToGoalMeters.HasValue)
{
distanceToCoverMm = (curDir.distanceToGoalMeters.Value - FollowDirectionTargetDistanceToGoalMeters.Value) * 1000.0d;
//Tracer.Trace("Follow Direction: distanceToCoverMm=" + distanceToCoverMm);
// call Linear Speed PID controller:
_state.followDirectionPidControllerLinearSpeed.Update(distanceToCoverMm, nowTicks);
linearPidRate = _state.followDirectionPidControllerLinearSpeed.CalculateControl();
}
//Tracer.Trace("Follow Direction: steepTurn=" + steepTurn + " turnRelative=" + turnRelative + " TargetDistanceToGoal=" + FollowDirectionTargetDistanceToGoalMeters.Value + " distanceToGoal=" + curDir.distanceToGoalMeters
// + " distanceToCover=" + distanceToCoverMm + " PID: angularPidRate=" + angularPidRate + " linearPidRate=" + linearPidRate);
double absFollowDirectionMaxVelocityMmSec = Math.Abs(FollowDirectionMaxVelocityMmSec);
double linearPower = steepTurn || !linearPidRate.HasValue ? 0.0d : (linearPidRate.Value * absFollowDirectionMaxVelocityMmSec / 1000000.0d); // power to speedMmSec ratio is 1000, linear PID rate is within the range -1000...+1000
// convert back to left/right power, which must be within the range -1.0 ... +1.0 and will be limited to this range by SetDrivePower()
// angular pid rate is within the range +-MaxPidValue in the followDirectionPidController
double angularPower = (angularPidRate / _state.followDirectionPidControllerAngularSpeed.MaxPidValue) * ModerateTurnPower;
double leftSpeed = (linearPower + angularPower) * 1000.0d;
double rightSpeed = (linearPower - angularPower) * 1000.0d;
//Tracer.Trace("Follow Direction: angularPower=" + angularPower + " linearPower=" + linearPower);
double wheelSpeedLimitMmSec = absFollowDirectionMaxVelocityMmSec < 0.01d ? MaximumForwardVelocityMmSec : absFollowDirectionMaxVelocityMmSec;
SetDriveSpeed(
Math.Sign(leftSpeed) * Math.Min(Math.Abs(leftSpeed), wheelSpeedLimitMmSec),
Math.Sign(rightSpeed) * Math.Min(Math.Abs(rightSpeed), wheelSpeedLimitMmSec)
);
}
else //if (!performedAvoidCollision)
{
// We are not tasked by Strategy with turning or driving. Strategy could be unsure or unable to do anything, collision state is unknown.
// We still might be moving, and if that's dangerous - we need to stop.
bool canMove = PerformAvoidCollision(null);
if (!canMove)
{
StopMoving();
_state.MovingState = MovingState.Unable;
_state.Countdown = 0; // 0 = immediate response
}
}
}
/// <summary>
/// recalculates relPosition
/// </summary>
/// <param name="myPos">usually robot position</param>
/// <param name="myDir">usually robot direction</param>
public void updateRelPosition(GeoPosition myPos, Direction myDir)
{
this.relPosition = new RelPosition(this.directionTo(myPos, myDir), this.distanceTo(myPos));
}
