protected void setGuiCurrentParkingSensor(proxibrick.ParkingSensorDataDssSerializable dir) { if (_mainWindow != null) { ccrwpf.Invoke invoke = new ccrwpf.Invoke( delegate() { _mainWindow.CurrentParkingSensor = new ParkingSensorData() { TimeStamp = dir.TimeStamp.Ticks, parkingSensorMetersLB = dir.parkingSensorMetersLB, parkingSensorMetersLF = dir.parkingSensorMetersLF, parkingSensorMetersRB = dir.parkingSensorMetersRB, parkingSensorMetersRF = dir.parkingSensorMetersRF }; } ); wpfServicePort.Post(invoke); Arbiter.Activate(TaskQueue, invoke.ResponsePort.Choice( s => { }, // delegate for success ex => { } //Tracer.Trace(ex) // delegate for failure )); } }
void trpbUpdateParkingSensorNotification(proxibrick.UpdateParkingSensorData update) { //LogInfo("DriveBehaviorServiceBase: trpbUpdateParkingSensorNotification()"); //Tracer.Trace("DriveBehaviorServiceBase: trpbUpdateParkingSensorNotification()"); if (!_mapperVicinity.robotState.ignoreParkingSensor) { try { proxibrick.ParkingSensorDataDssSerializable pds = update.Body; _state.MostRecentParkingSensor = pds; updateMapperWithOdometryData(); updateMapperWithParkingSensorData(pds); setGuiCurrentParkingSensor(pds); if (!_doUnitTest) { Decide(SensorEventSource.SonarDirected); } } catch (Exception exc) { Tracer.Trace("trpbUpdateParkingSensorNotification() - " + exc); } } }
private static double[] angles4 = { 77.5d, 102.5d, 257.5d, -77.5d }; // four Parking Sensor heads, two looking forward, two - backwards at a slight angle protected void updateMapperWithParkingSensorData(proxibrick.ParkingSensorDataDssSerializable psData) { double[] arrangedForMapper = new double[4]; arrangedForMapper[0] = psData.parkingSensorMetersRB; arrangedForMapper[1] = psData.parkingSensorMetersLB; arrangedForMapper[2] = psData.parkingSensorMetersLF; arrangedForMapper[3] = psData.parkingSensorMetersRF; for (int i = 0; i < arrangedForMapper.GetLength(0); i++) { double rangeMeters = arrangedForMapper[i]; if (rangeMeters < 2.2d) { rangeMeters += robotCornerDistanceMeters; // sensor is on the corner, adjust for that double relBearing = angles4[i] + 90.0d; GeoPosition pos1 = (GeoPosition)_mapperVicinity.robotPosition.Clone(); pos1.translate(new Direction() { heading = _mapperVicinity.robotDirection.heading, bearingRelative = relBearing }, new Distance(rangeMeters)); DetectedObstacle dobst1 = new DetectedObstacle() { geoPosition = pos1, firstSeen = psData.TimeStamp.Ticks, lastSeen = psData.TimeStamp.Ticks, detectorType = DetectorType.SONAR_DIRECTED, objectType = DetectedObjectType.Obstacle }; dobst1.SetColorByType(); lock (_mapperVicinity) { _mapperVicinity.Add(dobst1); } } } }
void trpbUpdateParkingSensorNotification(proxibrick.UpdateParkingSensorData update) { //LogInfo("DriveBehaviorServiceBase: trpbUpdateParkingSensorNotification()"); //Tracer.Trace("DriveBehaviorServiceBase: trpbUpdateParkingSensorNotification()"); try { proxibrick.ParkingSensorDataDssSerializable pds = update.Body; state.MostRecentParkingSensor = pds; // // Inform subscribed services that the state has changed. // //submgrPort.Post(new submgr.Submit(state, DsspActions.ReplaceRequest)); } catch (Exception exc) { Tracer.Trace("trpbUpdateParkingSensorNotification() - " + exc); } }
/// <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; }