public async Task <bool> Put(RoutePlan routePlan, string CompID, string EmplID, int CustID, string DatePlan) { string url = string.Format("http://webapi.local/api/RoutePlan/{0},{1},{2},{3}", CompID, EmplID, CustID, DatePlan); var json = JsonConvert.SerializeObject(routePlan); HttpContent content = new StringContent(json, Encoding.UTF8, "application/json"); HttpRequestMessage message = new HttpRequestMessage(HttpMethod.Put, url) { Content = content }; using (HttpResponseMessage response = await APIHelper.ApiClient.SendAsync(message)) { if (response.IsSuccessStatusCode) { return(true); } else { return(false); } } }
/// <summary> /// Transitions to the most appropriate state. /// </summary> /// <param name="distance">closest obstacle in corridor ahead</param> private void UpdateMovingState(RoutePlan plan) { LogInfo("TrackRoamerBehaviorsService:UpdateMovingState() Countdown=" + _state.Countdown + " MovingState=" + _state.MovingState); // at this point constraints and allowable speeds and distances are known and stored in _state.collisionState // we must use them while executing the plan. //if (_state.Countdown > 0) //{ // _state.Countdown--; // //_state.Countdown = 0; //} //else // based on MetaState, go to the MetaState...() method - which may change the MovingState appropriately if (_state.IsUnknown || _state.MovingState == MovingState.Unable) { MetaStateStartMapping(plan); } else if (_state.IsMoving) { MetaStateMove(plan); } else if (_state.IsMapping) { MetaStateMap(plan); } }
/// <summary> /// If the robot is mapping and there is sufficient open space directly ahead, enter this space. /// </summary> /// <param name="distance"></param> private void EnterOpenSpace(RoutePlan plan) { if (_state.IsMapping && plan != null && plan.isGoodPlan) { // at this point constraints and allowable speeds and distances are known and stored in _state.collisionState // we must use them while executing the plan. int?distance = (int)((double)plan.closestObstacleAlongBestPathMeters * 1000.0d); if (distance > SafeDistanceMm) { // We are mapping but can see plenty of free space ahead. // The robot should go into this space. LogInfo("TrackRoamerBehaviorsService: EnterOpenSpace() - while mapping, distance greater than SafeDistance=" + SafeDistanceMm); string message = "open space."; // "while mapping - detected open space; entering..."; Talker.Say(5, message); LogHistory(5, message); setMovingStateDetail(message); StopTurning(); _state.MovingState = MovingState.FreeForwards; _state.Countdown = 4; } } }
/// <summary> /// just turn where the plan wants you to; we are calling with good plan (heading) and no legMeters. /// </summary> /// <param name="plan"></param> private void TurnToGoal(RoutePlan plan) { // at this point constraints and allowable speeds and distances are known and stored in _state.collisionState // we must use them while executing the plan. _state.NewHeading = (int)Math.Round((double)plan.bestHeadingRelative(_mapperVicinity)); setMovingStateDetail("TurnToGoal to NewHeading=" + _state.NewHeading); AdjustHeading(); }
public void planRoute() { MapperVicinity mv = mapperViewControl1.CurrentMapper; RoutePlanner routePlanner = mapperViewControl1.CurrentRoutePlanner; RoutePlan plan = routePlanner.planRoute(); bestHeadingLabel.Content = "Best Heading abs=" + (plan.bestHeading.HasValue ? Math.Round((double)plan.bestHeading).ToString() : "N/A") + " rel (turn)=" + (plan.bestHeading.HasValue ? Math.Round((double)plan.bestHeading).ToString() : "N/A"); closestObstacleLabel.Content = "ClosestObstacle at " + (plan.closestObstacleMeters.HasValue ? (Math.Round((double)plan.closestObstacleMeters, 2).ToString() + " m") : "N/A"); legLengthLabel.Content = "Leg Length = " + (plan.legMeters.HasValue ? (Math.Round((double)plan.legMeters, 2).ToString() + " m") : "N/A (turn only)"); timeSpentLabel.Content = plan.timeSpentPlanning.ToString(); mapperViewControl1.RedrawMap(); }
/// <summary> /// If the robot is moving or intends to move, and an obstacle is too close, return false. The caller must take care to stop the movement. /// You can then map the environment for a way around it, or see what options are in _state.collisionState /// </summary> /// <param name="distance"></param> private bool PerformAvoidCollision(RoutePlan plan, double?intendedVelocity = null) { computeCollisionState(plan, intendedVelocity); setCollisionLights(); string messageCollisionState = _state.collisionState.message; bool mustStop = false; if (_state.IsMoving) { // we may come here with just proximity and other data, but not laser data. if (_state.collisionState.canMoveByPlan.HasValue && !_state.collisionState.canMoveByPlan.Value) { // // We are moving and there is something less than < ObstacleDistance> // millimeters from the center of the robot (within field of view) on the planned leg. STOP. // LogInfo("TrackRoamerBehaviorsService: AvoidCollision() - best plan obstacle - closer than ObstacleDistance=" + ObstacleDistanceMm); mustStop = true; } } setMovingStateDetail(messageCollisionState); if (_state.collisionState.mustStop || mustStop) { //string messageToSay = "stop."; // "avoid collision - must stop"; //if ((DateTime.Now - lastStopAnnounced).TotalMilliseconds > 3000.0d) //{ // Talker.Say(5, messageToSay); // lastStopAnnounced = DateTime.Now; //} LogHistory(5, "avoid collision - must stop"); return(false); // has to stop } else { return(true); // does not have to stop } }
private const double DrawTopInterval = 1.0d; // allow thinking only after so many seconds /// <summary> /// prepares HTTP graphics and planner, can call _routePlanner.planRoute() if doPlanRoute is true /// </summary> /// <param name="doPlanRoute"></param> private void initTactics(bool doPlanRoute) { bool canDrawTop = (DateTime.Now - lastDrawTop).TotalSeconds > DrawTopInterval; if (currentStatusGraphics != null) { currentStatusGraphics.Dispose(); currentStatusGraphics = null; } // we limit drawing to the cycles when laser data comes in, to limit frequency of drawing: if (canDrawTop) { lastDrawTop = DateTime.Now; currentStatusGraphics = new StatusGraphics(); } // ===== we can start writing to currentStatusGraphics bitmaps at will here. //Tracer.Trace("calling planRoute()"); _routePlanner.ObstacleDistanceMeters = ((double)ObstacleDistanceMm) / 1000.0d * 1.5d; if (doPlanRoute) { _currentRoutePlan = _routePlanner.planRoute(); } else { _currentRoutePlan = null; } if (canDrawTop) { // draw laser sweep data and basic markings: int fieldOfView = (int)Math.Round(_routePlanner.sweepAngleNormal / 2.0d); // to either side //Tracer.Trace("calling GenerateTop()"); GenerateTop(_laserData, fieldOfView, _currentRoutePlan); } sPlan = "no good plan"; }
/* Creates a route from 4350 Still Creek Dr to Langley BC with highways disallowed */ private void CreateRoute() { /* Initialize a CoreRouter */ CoreRouter coreRouter = new CoreRouter(); /* Initialize a RoutePlan */ RoutePlan routePlan = new RoutePlan(); /* * Initialize a RouteOption. HERE Mobile SDK allow users to define their own parameters for the * route calculation,including transport modes,route types and route restrictions etc.Please * refer to API doc for full list of APIs */ RouteOptions routeOptions = new RouteOptions(); /* Other transport modes are also available e.g Pedestrian */ routeOptions.SetTransportMode(RouteOptions.TransportMode.Car); /* Disable highway in this route. */ routeOptions.SetHighwaysAllowed(false); /* Calculate the shortest route available. */ routeOptions.SetRouteType(RouteOptions.Type.Shortest); /* Calculate 1 route. */ routeOptions.SetRouteCount(1); /* Finally set the route option */ routePlan.SetRouteOptions(routeOptions); /* Define waypoints for the route */ // START: Daimler Fleetboard RouteWaypoint startPoint = new RouteWaypoint(new GeoCoordinate(48.7244153, 9.1161991)); // END: Biergarten RouteWaypoint destination = new RouteWaypoint(new GeoCoordinate(48.701356, 9.1393803)); /* Add both waypoints to the route plan */ routePlan.AddWaypoint(startPoint); routePlan.AddWaypoint(destination); /* Trigger the route calculation,results will be called back via the listener */ coreRouter.CalculateRoute(routePlan, new RouterListener(this)); }
/// <summary> /// see if current plan chain has a plan good enough to satisfy collisionState constraints; /// </summary> /// <param name="plan"></param> private RoutePlan critiquePlanChain(RoutePlan plan) { RoutePlan ret = plan; if (plan != null) { double?bestHeadingRelative = plan.bestHeadingRelative(_mapperVicinity); if (bestHeadingRelative.HasValue) { if (!_state.collisionState.canTurnLeft && bestHeadingRelative < 0 || !_state.collisionState.canTurnRight && bestHeadingRelative > 0) { Tracer.Trace("Unable to turn " + bestHeadingRelative + " degrees due to collisionState"); // TODO: see if any of plan.fallBackPlans can be used ret = null; } } } return(ret); // selected from initial plan chain }
/// <summary> /// Implements the "Moving" meta state, basically any transition and not in-place sequence. /// </summary> /// <param name="distance">closest obstacle in corridor ahead</param> private void MetaStateMove(RoutePlan plan) { LogInfo("TrackRoamerBehaviorsService: MetaStateMove()"); Talker.Say(4, "Move - " + _state.MovingState); // at this point constraints and allowable speeds and distances are known and stored in _state.collisionState // we must use them while executing the plan. switch (_state.MovingState) { case MovingState.AdjustHeading: setMovingStateDetail("MetaStateMove - AdjustHeading to NewHeading=" + _state.NewHeading); AdjustHeading(); break; case MovingState.FreeForwards: setMovingStateDetail("MetaStateMove - FreeForwards"); AdjustDirectionAndVelocity(plan); break; case MovingState.InTransition: case MovingState.Unknown: // inTransition means "while executing TurnAndMoveForward() command" (see base). // TurnAndMoveForward() may for a moment leave the state in "Unknown", do not treat it as termination. // this allows current transition to complete, without exiting the "Moving" metastate break; case MovingState.BumpedBackingUp: setMovingStateDetail("MetaStateMove - BumpedBackingUp"); break; default: LogError("TrackRoamerBehaviorsService:MetaStateMove() called in illegal state - " + _state.MovingState); break; } }
/// <summary> /// evaluates proximity and parking sensor data and translates all of it into /// actionable restrictions in CollisionState object /// </summary> public void computeCollisionState(RoutePlan plan, double?intendedVelocity) { CollisionState cs = _state.collisionState; //cs.initRestrictive(); cs.initPermissive(FreeDistanceMm, MaximumForwardVelocityMmSec, MaximumBackwardVelocityMmSec); // initialize to the maximums: canMoveForwardDistanceM = FreeDistanceMm * 2.0d / 1000.0d; canMoveBackwardsDistanceM = FreeDistanceMm * 2.0d / 1000.0d; //Tracer.Trace("IR ffl=" + _state.MostRecentProximity.mfl + "m PS MetersLF=" + _state.MostRecentParkingSensor.parkingSensorMetersLF + "m"); if (intendedVelocity == null) { intendedVelocity = _state.Velocity; } bool isMovingForward = intendedVelocity > 0.01d; // m/s bool isMovingBackwards = intendedVelocity < -0.01d; int?distance = null; // mm if (plan != null && plan.isGoodPlan) { double?bestHeadingRelative = plan.bestHeadingRelative(_mapperVicinity); if (bestHeadingRelative != null && bestHeadingRelative.Value > -45.0d && bestHeadingRelative.Value < 45.0d) { distance = (int)(plan.legMeters.Value * 1000.0d); if (distance < ObstacleDistanceMm) { cs.canMoveByPlan = false; } else { cs.canMoveByPlan = true; } } } DateTime sensorInvalidateTime = DateTime.Now.AddSeconds(-ignoreOldSensorReadingsIntervalSec); // ignore sensor readings that are older than this time StringBuilder msb = new StringBuilder(); // build the message here // we compute full state every time, but format the message so it is relevant to the direction we move to. // forward sensors: ------------------------------------------------------------------------------------------- proxibrick.ProximityDataDssSerializable MostRecentProximity = _state.MostRecentProximity; proxibrick.ParkingSensorDataDssSerializable MostRecentParkingSensor = _state.MostRecentParkingSensor; if (MostRecentProximity != null && MostRecentProximity.TimeStamp > sensorInvalidateTime) { double mfl = MostRecentProximity.mfl; if (mfl < proxiTreshold) { if (isMovingForward) { msb.AppendFormat("Front Left Side proximity detected obstacle at {0}m; ", mfl); } cs.canTurnLeft &= mfl > proxiTresholdTurn1; if (mfl < canMoveForwardDistanceM) { canMoveForwardDistanceM = mfl; } } double mffl = MostRecentProximity.mffl; if (mffl < proxiTreshold) { if (isMovingForward) { msb.AppendFormat("FF_Left proximity detected obstacle at {0}m; ", mffl); } //cs.canTurnLeft &= mffl > proxiTresholdTurn2; if (mffl < canMoveForwardDistanceM) { canMoveForwardDistanceM = mffl; } } double mffr = MostRecentProximity.mffr; if (mffr < proxiTreshold) { if (isMovingForward) { msb.AppendFormat("FF_Right proximity detected obstacle at {0}m; ", mffr); } //cs.canTurnRight &= mffr > proxiTresholdTurn2; if (mffr < canMoveForwardDistanceM) { canMoveForwardDistanceM = mffr; } } double mfr = MostRecentProximity.mfr; if (mfr < proxiTreshold) { if (isMovingForward) { msb.AppendFormat("Front Right Side proximity detected obstacle at {0}m; ", mfr); } cs.canTurnRight &= mfr > proxiTresholdTurn1; if (mfr < canMoveForwardDistanceM) { canMoveForwardDistanceM = mfr; } } } if (MostRecentParkingSensor != null && MostRecentParkingSensor.TimeStamp > sensorInvalidateTime) { double parkingSensorMetersLF = MostRecentParkingSensor.parkingSensorMetersLF; if (parkingSensorMetersLF < psiTreshold) { if (isMovingForward) { msb.AppendFormat("Front Left parking sensor detected obstacle at {0}m; ", parkingSensorMetersLF); } if (parkingSensorMetersLF < canMoveForwardDistanceM) { canMoveForwardDistanceM = parkingSensorMetersLF; } } double parkingSensorMetersRF = MostRecentParkingSensor.parkingSensorMetersRF; if (parkingSensorMetersRF < psiTreshold) { if (isMovingForward) { msb.AppendFormat("Front Right parking sensor detected obstacle at {0}m; ", parkingSensorMetersRF); } if (parkingSensorMetersRF < canMoveForwardDistanceM) { canMoveForwardDistanceM = parkingSensorMetersRF; } } } // backward sensors: ------------------------------------------------------------------------------------------- if (MostRecentProximity != null && MostRecentProximity.TimeStamp > sensorInvalidateTime) { double mbl = MostRecentProximity.mbl; if (mbl < proxiTreshold) { if (isMovingBackwards) { msb.AppendFormat("Back Left Side proximity detected obstacle at {0}m; ", mbl); } cs.canTurnRight &= mbl > proxiTresholdTurn1; if (mbl < canMoveBackwardsDistanceM) { canMoveBackwardsDistanceM = mbl; } } double mbbl = MostRecentProximity.mbbl; if (mbbl < proxiTreshold) { if (isMovingBackwards) { msb.AppendFormat("BB_Left proximity detected obstacle at {0}m; ", mbbl); } //cs.canTurnRight &= mbbl > proxiTresholdTurn2; if (mbbl < canMoveBackwardsDistanceM) { canMoveBackwardsDistanceM = mbbl; } } double mbbr = MostRecentProximity.mbbr; if (mbbr < proxiTreshold) { if (isMovingBackwards) { msb.AppendFormat("BB_Right proximity detected obstacle at {0}m; ", mbbr); } //cs.canTurnLeft &= mbbr > proxiTresholdTurn2; if (mbbr < canMoveBackwardsDistanceM) { canMoveBackwardsDistanceM = mbbr; } } double mbr = MostRecentProximity.mbr; if (mbr < proxiTreshold) { if (isMovingBackwards) { msb.AppendFormat("Back Right Side proximity detected obstacle at {0}m; ", mbr); } cs.canTurnLeft &= mbr > proxiTresholdTurn1; if (mbr < canMoveBackwardsDistanceM) { canMoveBackwardsDistanceM = mbr; } } } if (MostRecentParkingSensor != null && MostRecentParkingSensor.TimeStamp > sensorInvalidateTime) { double parkingSensorMetersLB = MostRecentParkingSensor.parkingSensorMetersLB; if (parkingSensorMetersLB < psiTreshold) { if (isMovingBackwards) { msb.AppendFormat("Back Left parking sensor detected obstacle at {0}m; ", parkingSensorMetersLB); } if (parkingSensorMetersLB < canMoveBackwardsDistanceM) { canMoveBackwardsDistanceM = parkingSensorMetersLB; } } double parkingSensorMetersRB = MostRecentParkingSensor.parkingSensorMetersRB; if (parkingSensorMetersRB < psiTreshold) { if (isMovingBackwards) { msb.AppendFormat("Back Right parking sensor detected obstacle at {0}m; ", parkingSensorMetersRB); } if (parkingSensorMetersRB < canMoveBackwardsDistanceM) { canMoveBackwardsDistanceM = parkingSensorMetersRB; } } } if (_state.MostRecentWhiskerLeft) { msb.Append("Left Whisker; "); canMoveForwardDistanceM = 0.0d; cs.canMoveForward = false; cs.canTurnRight = false; cs.canTurnLeft = false; } if (_state.MostRecentWhiskerRight) { msb.Append("Right Whisker; "); canMoveForwardDistanceM = 0.0d; cs.canMoveForward = false; cs.canTurnRight = false; cs.canTurnLeft = false; } adjustNearCollisionSpeed(canMoveForwardDistanceM, true); adjustNearCollisionSpeed(canMoveBackwardsDistanceM, false); // compute the "mustStop" and "message": if (isMovingForward) { cs.mustStop = !cs.canMoveForward; } else if (isMovingBackwards) { cs.mustStop = !cs.canMoveBackwards; } if (msb.Length > 0 || cs.mustStop) { if (isMovingForward) { msb.Insert(0, "While moving forward: "); } else if (isMovingBackwards) { msb.Insert(0, "While moving backwards: "); } if (cs.mustStop) { msb.Insert(0, "!! MUST STOP !! "); } } else { if (isMovingForward) { msb.Append("Can continue forward: "); } else if (isMovingBackwards) { msb.Append("Can continue backwards: "); } else { msb.Append("Can move: "); } } msb.AppendFormat(" FWD: {0} to {1} mm at {2:F0} mm/s BKWD: {3} to {4} mm at {5:F0} mm/s LEFT:{6} RIGHT:{7}", cs.canMoveForward, cs.canMoveForwardDistanceMm, cs.canMoveForwardSpeedMms, cs.canMoveBackwards, cs.canMoveBackwardsDistanceMm, cs.canMoveBackwardsSpeedMms, cs.canTurnLeft, cs.canTurnRight); if (cs.canMoveByPlan.HasValue && !cs.canMoveByPlan.Value) { msb.AppendFormat("; best plan obstacle too close at {0} mm; ", distance); } string message = msb.ToString(); //if (!string.Equals(message, _lastMessage)) //{ // Tracer.Trace(message); // _lastMessage = message; //} cs.message = message; }
protected void GenerateTop(sicklrf.State laserData, int fieldOfView, RoutePlan plan) { //lock (lockStatusGraphics) //{ if (currentStatusGraphics != null) { bool haveLaser = laserData != null && laserData.DistanceMeasurements != null && (DateTime.Now - _laserData.TimeStamp).TotalSeconds < 2.0d; //Bitmap bmp = (_state.MovingState == MovingState.MapSouth) ? currentStatusGraphics.statusBmp : currentStatusGraphics.northBmp; //Bitmap bmp = currentStatusGraphics.statusBmp; Bitmap bmp = currentStatusGraphics.northBmp; lock (bmp) { using (Graphics g = Graphics.FromImage(bmp)) { g.Clear(Color.LightGray); List <Lbl> labels = new List <Lbl>(); if (haveLaser) { double angularOffset = -90 + laserData.AngularRange / 2.0; double piBy180 = Math.PI / 180.0; double halfAngle = laserData.AngularResolution / 2.0; double drangeMax = 0.0d; GraphicsPath path = new GraphicsPath(); // make two passes, drawing laser data and label every 20th range: for (int pass = 1; pass <= 2; pass++) { for (int i = 0; i < laserData.DistanceMeasurements.Length; i++) { int range = laserData.DistanceMeasurements[i]; if (range > 0 && range < 8192) { double angle = i * laserData.AngularResolution - angularOffset; double lowAngle = (angle - halfAngle) * piBy180; double highAngle = (angle + halfAngle) * piBy180; double drange = range * StatusGraphics.scale; float lx = (float)(StatusGraphics.xCenter + drange * Math.Cos(lowAngle)); float ly = (float)(StatusGraphics.xCenter - drange * Math.Sin(lowAngle)); float hx = (float)(StatusGraphics.xCenter + drange * Math.Cos(highAngle)); float hy = (float)(StatusGraphics.xCenter - drange * Math.Sin(highAngle)); if (pass == 1) { // on the first pass just add lines to the Path and calculate the max range: if (i == 0) { path.AddLine(StatusGraphics.xCenter, StatusGraphics.imageHeight, lx, ly); } path.AddLine(lx, ly, hx, hy); drangeMax = Math.Max(drangeMax, drange); } else { // on the second pass draw the perimeter and label every 20th range: g.DrawLine(Pens.DarkBlue, lx, ly, hx, hy); if (i > 0 && i % 20 == 0 && i < laserData.DistanceMeasurements.Length - 10) { float llx = (float)(StatusGraphics.xCenter + drangeMax * 1.3f * Math.Cos(lowAngle)); float lly = (float)(StatusGraphics.xCenter - drangeMax * 1.3f * Math.Sin(lowAngle)); double roundRange = Math.Round(range / 1000.0d, 1); // meters string str = "" + roundRange; labels.Add(new Lbl() { label = str, lx = llx, ly = lly, brush = Brushes.Black }); } } } } if (pass == 1) { // draw the laser sweep on the first pass: g.FillPath(Brushes.White, path); } } } // draw important decision-influencing boundaries: float startAngle = -150.0f; float sweepAngle = 120.0f; // the "stop moving" distance: float radius = (float)(ObstacleDistanceMm * StatusGraphics.scale); //g.DrawRectangle(Pens.Red, xCenter - radius, imageHeight - radius, radius * 2, radius * 2); g.DrawArc(Pens.Red, StatusGraphics.xCenter - radius, StatusGraphics.imageHeight - radius, radius * 2, radius * 2, startAngle, sweepAngle); rayLabel("stop", labels, ObstacleDistanceMm, -60.0d, Brushes.Red); // the "slow down" distance: radius = (float)(AwareOfObstacleDistanceMm * StatusGraphics.scale); g.DrawArc(Pens.Orange, StatusGraphics.xCenter - radius, StatusGraphics.imageHeight - radius, radius * 2, radius * 2, startAngle, sweepAngle); rayLabel("slow", labels, AwareOfObstacleDistanceMm, -60.0d, Brushes.Orange); // the "stop mapping, enter the open space" distance radius = (float)(SafeDistanceMm * StatusGraphics.scale); g.DrawArc(Pens.LightBlue, StatusGraphics.xCenter - radius, StatusGraphics.imageHeight - radius, radius * 2, radius * 2, startAngle, sweepAngle); rayLabel("enter", labels, SafeDistanceMm, -60.0d, Brushes.LightBlue); // the "max velocity" distance: radius = (float)(FreeDistanceMm * StatusGraphics.scale); g.DrawArc(Pens.Green, StatusGraphics.xCenter - radius, StatusGraphics.imageHeight - radius, radius * 2, radius * 2, startAngle, sweepAngle); rayLabel("free", labels, FreeDistanceMm, -60.0d, Brushes.Green); // the fieldOfView arc: radius = (float)(StatusGraphics.imageHeight - 10); g.DrawArc(Pens.LimeGreen, StatusGraphics.xCenter - radius, StatusGraphics.imageHeight - radius, radius * 2, radius * 2, (float)(-90 - fieldOfView), (float)(fieldOfView * 2)); rayLabel("field of view", labels, (StatusGraphics.imageHeight + 5) / StatusGraphics.scale, 10.0d, Brushes.LimeGreen); // now draw the robot. Trackroamer is 680 mm wide. float botHalfWidth = (float)(680 / 2.0d * StatusGraphics.scale); DrawHelper.drawRobotBoundaries(g, botHalfWidth, StatusGraphics.imageWidth / 2, StatusGraphics.imageHeight); // debug- draw a ray pointing to predefined direction; left is positive, right is negative: //double rayLength = 2000.0d; //rayLine("20", g, rayLength, 20.0d, Pens.Cyan, Brushes.DarkCyan); //rayLine("-20", g, rayLength, -20.0d, Pens.Blue, Brushes.Blue); //rayLine("70", g, rayLength, 70.0d, Pens.Red, Brushes.Red); //rayLine("-70", g, rayLength, -70.0d, Pens.Green, Brushes.Green); // draw laser's time stamp: if (haveLaser) { TimeSpan howOld = DateTime.Now - laserData.TimeStamp; g.DrawString(laserData.TimeStamp.ToString() + " (" + howOld + ")", StatusGraphics.fontBmp, Brushes.Black, 0, 0); } HistoryItem latestDecisionsHistory = StatusGraphics.HistoryDecisions.Peek(); HistoryItem latestSaidHistory = Talker.HistorySaid.Peek(); if (latestSaidHistory != null) { g.DrawString(latestSaidHistory.message, StatusGraphics.fontBmpL, Brushes.Black, StatusGraphics.imageWidth / 2 + 80, StatusGraphics.imageHeight + StatusGraphics.extraHeight - 20); } if (latestDecisionsHistory != null) { g.DrawString(latestDecisionsHistory.message, StatusGraphics.fontBmpL, Brushes.Black, StatusGraphics.imageWidth / 2 + 80, StatusGraphics.imageHeight + StatusGraphics.extraHeight - 40); } g.DrawString(_state.MovingState.ToString(), StatusGraphics.fontBmpL, Brushes.Black, StatusGraphics.imageWidth / 2 - 40, StatusGraphics.imageHeight + StatusGraphics.extraHeight - 20); // draw distance labels over all other graphics: foreach (Lbl lbl in labels) { g.DrawString(lbl.label, StatusGraphics.fontBmp, lbl.brush, lbl.lx, lbl.ly + 20); } // a 200W x 400H rectangle: Rectangle drawRect = new Rectangle(StatusGraphics.imageWidth / 2 - StatusGraphics.extraHeight, StatusGraphics.imageHeight - StatusGraphics.extraHeight * 2, StatusGraphics.extraHeight * 2, StatusGraphics.extraHeight * 4); if (_state.MostRecentProximity != null) { // the MostRecentProximity class here comes from the proxy, and does not have arrangedForDrawing member. Restore it: double[] arrangedForDrawing = new double[8]; arrangedForDrawing[0] = _state.MostRecentProximity.mbr; arrangedForDrawing[1] = _state.MostRecentProximity.mbbr; arrangedForDrawing[2] = _state.MostRecentProximity.mbbl; arrangedForDrawing[3] = _state.MostRecentProximity.mbl; arrangedForDrawing[4] = _state.MostRecentProximity.mfr; arrangedForDrawing[5] = _state.MostRecentProximity.mffr; arrangedForDrawing[6] = _state.MostRecentProximity.mffl; arrangedForDrawing[7] = _state.MostRecentProximity.mfl; // draw a 200x400 image of IR proximity sensors: DrawHelper.drawProximityVectors(g, drawRect, arrangedForDrawing, 1); } if (_state.MostRecentParkingSensor != null) { // the MostRecentParkingSensor class here comes from the proxy, and does not have arrangedForDrawing member. Restore it: double[] arrangedForDrawing = new double[4]; arrangedForDrawing[0] = _state.MostRecentParkingSensor.parkingSensorMetersRB; arrangedForDrawing[1] = _state.MostRecentParkingSensor.parkingSensorMetersLB; arrangedForDrawing[2] = _state.MostRecentParkingSensor.parkingSensorMetersLF; arrangedForDrawing[3] = _state.MostRecentParkingSensor.parkingSensorMetersRF; // draw a 200x400 image of parking sensors: DrawHelper.drawProximityVectors(g, drawRect, arrangedForDrawing, 2); } if (plan != null && plan.isGoodPlan) { // right turn - positive, left turn - negative, expressed in degrees: int bestHeadingInt = (int)Math.Round((double)plan.bestHeadingRelative(_mapperVicinity)); double bestHeadingDistance = plan.legMeters.Value * 1000.0d; using (Pen dirPen = new Pen(Color.Green, 5.0f)) { dirPen.EndCap = LineCap.ArrowAnchor; rayLine("best:" + bestHeadingInt, g, bestHeadingDistance * 1.1d, (double)bestHeadingInt, dirPen, Brushes.Green); } //int xLbl = 10; //int yLbl = bmp.Height - 40; //g.DrawString(comment, StatusGraphics.fontBmpL, Brushes.Green, xLbl, yLbl); } if (_mapperVicinity.robotDirection.bearingRelative.HasValue) { int goalBearingInt = (int)Math.Round((double)_mapperVicinity.robotDirection.bearingRelative); double goalDistance = 3000.0d; using (Pen dirPen = new Pen(Color.LimeGreen, 2.0f)) { dirPen.EndCap = LineCap.ArrowAnchor; rayLine("goal:" + goalBearingInt, g, goalDistance, (double)goalBearingInt, dirPen, Brushes.LimeGreen); } } } } } }
/// <summary> /// Transitions to "Mapping" meta state or "AdjustHeading" state depending on /// environment. /// </summary> /// <param name="distance">closest obstacle in corridor ahead</param> private void MetaStateStartMapping(RoutePlan plan) { int distance = 0; if (plan.legMeters.HasValue) { distance = (int)((double)plan.legMeters * 1000.0d); } LogInfo("TrackRoamerBehaviorsService: MetaStateStartMapping(distance=" + distance + ") ObstacleDistance=" + ObstacleDistanceMm); Talker.Say(5, "Mapping"); markNorthBitmap("Start Mapping"); StopMoving(); // at this point constraints and allowable speeds and distances are known and stored in _state.collisionState // we must use them while executing the plan. if (distance < ObstacleDistanceMm) { LogInfo(" distance < ObstacleDistance (not finding exit) _state.Mapped=" + _state.Mapped); _state.MovingState = MovingState.Recovering; LogInfo(" MovingState Recovering (not finding exit)"); //Talker.Say(5, "Recovering"); setMovingStateDetail("Recovering (not finding exit)"); /* * if (_state.Mapped) * { * // We have been mapping before but do not seem to * // have found anything. * _state.MovingState = MovingState.RandomTurn; * LogInfo(" requesting RandomTurn"); * Talker.Say(5, "random turn"); * setMovingStateDetail("Start Mapping (not finding exit) - requesting RandomTurn"); * } * else * { * _state.MovingState = MovingState.MapSurroundings; * LogInfo(" requesting MapSurroundings"); * Talker.Say(5, "map surroundings"); * setMovingStateDetail("Start Mapping (not finding exit) - requesting MapSurroundings"); * } * */ } else { LogInfo(" distance > ObstacleDistance (exit route found) CorridorWidthMapping=" + CorridorWidthMappingMm); //Talker.Say(5, "exit route found"); int legMm = Math.Min(ObstacleDistanceMm, distance); _state.NewHeading = (int)plan.bestHeadingRelative(_mapperVicinity); // right - positive, left - negative LogInfo("Leg: " + legMm + " mm NewHeading=" + _state.NewHeading); setMovingStateDetail("Start Mapping (exit route found) NewHeading=" + _state.NewHeading + "Leg: " + legMm + " mm"); _state.MovingState = MovingState.AdjustHeading; _state.Countdown = legMm / 50 + Math.Abs(_state.NewHeading / 10); LogInfo(" new Countdown=" + _state.Countdown); } }
/// <summary> /// Adjusts the velocity based on environment. /// </summary> /// <param name="distance">closest obstacle in corridor ahead</param> private void AdjustDirectionAndVelocity(RoutePlan plan) { LogInfo("TrackRoamerBehaviorsService: AdjustDirectionAndVelocity()"); // at this point constraints and allowable speeds and distances are known and stored in _state.collisionState // we must use them while executing the plan. int legDistanceMm = _state.collisionState.canMoveForwardDistanceMm; if (plan.legMeters.HasValue) { legDistanceMm = Math.Min(legDistanceMm, (int)((double)plan.legMeters * 1000.0d)); } // we come here when _state.MovingState == MovingState.FreeForwards _state.Mapped = false; //int bestHeading = FindBestHeading(_laserData, 0, (int)Math.Round(_state.Velocity * 1000.0d / 10.0d), CorridorWidthMoving, "FreeForwards - Adjust"); _state.NewHeading = (int)plan.bestHeadingRelative(_mapperVicinity); // right - positive, left - negative LogInfo(" bestHeading=" + _state.NewHeading + currentCompass); if (legDistanceMm > FreeDistanceMm) { SpawnIterator <TurnAndMoveParameters, Handler>( new TurnAndMoveParameters() { rotateAngle = (int)(((double)_state.NewHeading) * turnHeadingFactor), rotatePower = MaximumTurnPower, speed = (int)Math.Round(Math.Min(_state.collisionState.canMoveForwardSpeedMms, MaximumForwardVelocityMmSec)), desiredMovingState = MovingState.FreeForwards }, delegate() { }, TurnAndMoveForward); } else if (legDistanceMm > AwareOfObstacleDistanceMm) { SpawnIterator <TurnAndMoveParameters, Handler>( new TurnAndMoveParameters() { rotateAngle = (int)(((double)_state.NewHeading) * turnHeadingFactor), rotatePower = ModerateTurnPower, speed = (int)Math.Round(Math.Min(_state.collisionState.canMoveForwardSpeedMms, ModerateForwardVelocityMmSec)), desiredMovingState = MovingState.FreeForwards }, delegate() { }, TurnAndMoveForward); } else { SpawnIterator <TurnAndMoveParameters, Handler>( new TurnAndMoveParameters() { rotateAngle = (int)(((double)_state.NewHeading) * turnHeadingFactor), rotatePower = MinimumTurnPower, speed = (int)Math.Round(Math.Min(_state.collisionState.canMoveForwardSpeedMms, MinimumForwardVelocityMmSec)), desiredMovingState = MovingState.FreeForwards }, delegate() { _state.Countdown = Math.Abs(_state.NewHeading / 10); }, TurnAndMoveForward); } LogInfo(" new Countdown=" + _state.Countdown); }
private const double TurningWaitInterval = 1.0d; // wait for so many seconds after a turn before accepting a laser frame (should be close to full sweep cycle of the laser) #region PlanAndAvoid tactics private void TacticsPlanAndAvoid() { if (_currentRoutePlan.isGoodPlan) { // we have a good plan, try executing it: string sTurn = "no turn"; double?bestHeadingRelative = _currentRoutePlan.bestHeading - _mapperVicinity.robotDirection.heading; if (bestHeadingRelative.HasValue && bestHeadingRelative != 0.0d) { sTurn = "turn " + (bestHeadingRelative > 0.0d ? "right " : "left ") + bestHeadingRelative + " degrees"; } if (_currentRoutePlan.legMeters.HasValue) { sPlan = "plan: " + sTurn + " and go up to " + Math.Round((double)_currentRoutePlan.legMeters, 2) + " meters (obstacle at " + Math.Round((double)_currentRoutePlan.closestObstacleAlongBestPathMeters, 2) + " meters"; } else { sPlan = "plan: steep " + sTurn + " only (obstacle at " + Math.Round((double)_currentRoutePlan.closestObstacleAlongBestPathMeters, 2) + " meters)"; } Tracer.Trace(sPlan); // AvoidCollision and EnterOpenSpace have precedence over // all other state transitions and are thus handled first. bool canMove = PerformAvoidCollision(_currentRoutePlan); // may result in state set to MovingState.Unable // 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 RoutePlan bestPlan = critiquePlanChain(_currentRoutePlan); if (bestPlan == null) { _state.MovingState = MovingState.Unable; // no good plan in the chain } if (_state.MovingState != MovingState.Unknown && _state.MovingState != MovingState.Unable) { if (!_currentRoutePlan.legMeters.HasValue) { // plan requires a steep turn towards the goal: TurnToGoal(_currentRoutePlan); } else { // plan suggests moving and adjusting heading and speed: EnterOpenSpace(_currentRoutePlan); UpdateMovingState(_currentRoutePlan); } } else { // state - Unknown or Unable; will likely start mapping UpdateMovingState(_currentRoutePlan); } } else { // the plan is no good. Just avoid collision for now: Tracer.Trace(sPlan); bool canMove = PerformAvoidCollision(null); // may result in state set to MovingState.Unable (if mustStop) } }
/// <summary> /// Implements the "Mapping" meta state. /// </summary> /// <param name="distance">closest obstacle in corridor ahead</param> private void MetaStateMap(RoutePlan plan) { LogInfo("TrackRoamerBehaviorsService: MetaStateMap() MovingState=" + _state.MovingState); // at this point constraints and allowable speeds and distances are known and stored in _state.collisionState // we must use them while executing the plan. switch (_state.MovingState) { case MovingState.RandomTurn: LogInfo("MovingState.RandomTurn"); setMovingStateDetail("MetaStateMap - performing RandomTurn"); RandomTurn(); break; case MovingState.Recovering: LogInfo("MovingState.Recovering"); setMovingStateDetail("MetaStateMap - Recovering"); Recover(); break; /* * case MovingState.MapSurroundings: * _state.Mapped = true; * LogInfo("MovingState.MapSurroundings: Turning 180 deg to map"); * * setMovingStateDetail("MetaStateMap - turning 180 deg to map"); * * SpawnIterator<TurnAndMoveParameters, Handler>( * new TurnAndMoveParameters() { * speed = (int)Math.Round(MaximumBackwardVelocity), * rotatePower = ModerateTurnPower, * rotateAngle = 180, * desiredMovingState = MovingState.MapSouth * }, * delegate() { * _state.Countdown = 15; * }, * TurnAndMoveForward); * * break; * * case MovingState.MapSouth: * LogInfo("MovingState.MapSouth: mapping the View South"); * * Talker.Say(5, "mapping South"); * * setMovingStateDetail("MetaStateMap - mapping South"); * * // _state.South = _laserData; // needed for FindBestComposite() * * SpawnIterator<TurnAndMoveParameters, Handler>( * new TurnAndMoveParameters() { * speed = (int)Math.Round(MaximumBackwardVelocity), * rotatePower = ModerateTurnPower, * rotateAngle = 180, * desiredMovingState = MovingState.MapNorth * }, * delegate() { * _state.Countdown = 15; * }, * TurnAndMoveForward); * * break; * * case MovingState.MapNorth: * LogInfo("MovingState.MapNorth: mapping the View North"); * * Talker.Say(5, "mapping North"); * * //_state.NewHeading = FindBestComposite(_state.South, _laserData); * * _state.NewHeading = (int)plan.bestHeadingRelative(_mapperVicinity); * * LogInfo("Composite Map suggests turn to heading: " + _state.NewHeading); * * setMovingStateDetail("MetaStateMap - mapping North - Composite Map suggests turn: " + _state.NewHeading); * * _state.South = null; * _state.MovingState = MovingState.AdjustHeading; * break; */ case MovingState.InTransition: break; default: LogError("TrackRoamerBehaviorsService:MetaStateMap() called in illegal state - " + _state.MovingState); break; } }