void PhysicsContactNotificationHandler(physics.EntityContactNotification contact) { foreach (physics.ShapeContact sc in contact.ShapeContacts) { // look up shape involved in collision and check its one of the bumper shapes pxContactSensor.ContactSensor s; if (!_bumperShapeToSensorTable.TryGetValue(sc.LocalShape, out s)) { continue; } if (contact.Stage == physics.ContactNotificationStage.Started) { s.Pressed = true; } else if (contact.Stage == physics.ContactNotificationStage.Finished) { s.Pressed = false; } s.TimeStamp = DateTime.Now; // notification for individual sensor _subMgrPort.Post(new submgr.Submit(s, dssp.DsspActions.UpdateRequest)); } // send notification _subMgrPort.Post(new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest)); // reactivate notification handler Activate(Arbiter.Receive(false, _contactNotificationPort, PhysicsContactNotificationHandler)); }
/// <summary> /// Handle sensor update message from Scribbler /// </summary> public void SensorNotificationHandler(brick.Replace notify) { _state.LineDetected = (notify.Body.LineLeft || notify.Body.LineRight); if (_state.LineDetected) { int left = 0; int right = 0; if (notify.Body.LineLeft) { left = -1; } if (notify.Body.LineRight) { right = 1; } _state.LocationOfLine = left + right; } _state.TimeStamp = DateTime.Now; bool changed = true; if (changed) { //notify subscribers on any bumper pressed or unpressed _subMgrPort.Post(new submgr.Submit(_state, DsspActions.UpdateRequest)); } }
/// <summary> /// Handle sensor update message from Scribbler /// </summary> public void SensorNotificationHandler(brick.Replace notify) { //update state foreach (bumper.ContactSensor sensor in _state.Sensors) { bool newval = true; if (sensor.Name.ToUpper().Contains("LEFT")) { newval = !notify.Body.IRLeft; //NOTE: inverting logic here } else if (sensor.Name.ToUpper().Contains("RIGHT")) { newval = !notify.Body.IRRight; } else { LogError("Bumper name missmatch"); } bool changed = (sensor.Pressed != newval); sensor.TimeStamp = DateTime.Now; sensor.Pressed = newval; if (changed) { //notify subscribers on any bumper pressed or unpressed _subMgrPort.Post(new submgr.Submit(sensor, DsspActions.UpdateRequest)); } } }
public IEnumerator <ITask> ReplaceHandler(Replace replace) { replace.ResponsePort.Post(new dssp.DefaultReplaceResponseType()); _subMgrPort.Post(new submgr.Submit(_state, DsspActions.ReplaceRequest)); _state = replace.Body; yield break; }
/// <summary> /// Handle new measurement data from the LRF. /// </summary> /// <param name="measurement">Measurement Data</param> void MeasurementHandler(LinkMeasurement measurement) { //Tracer.Trace("SickLRF::MeasurementHandler()"); try { // // The SickLRF typically reports on either a 180 degrees or 100 degrees // field of vision. From the number of readings we can calculate the // Angular Range and Resolution. // switch (measurement.Ranges.Length) { case 181: // we always get here: _state.AngularRange = 180; _state.AngularResolution = 1; break; case 361: _state.AngularRange = 180; _state.AngularResolution = 0.5; break; case 101: _state.AngularRange = 100; _state.AngularResolution = 1; break; case 201: _state.AngularRange = 100; _state.AngularResolution = 0.5; break; case 401: _state.AngularRange = 100; _state.AngularResolution = 0.25; break; default: break; } _state.DistanceMeasurements = measurement.Ranges; _state.Units = measurement.Units; _state.TimeStamp = measurement.TimeStamp; _state.LinkState = "Measurement received"; // // Inform subscribed services that the state has changed. // _subMgrPort.Post(new submgr.Submit(_state, DsspActions.ReplaceRequest)); } catch (Exception e) { LogError(e); } }
public IEnumerator <ITask> ReplaceHandler(depthcam.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; }
public IEnumerator <ITask> SubscribeHandler(bumper.Subscribe subscribe) { yield return(Arbiter.Choice( SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort), delegate(SuccessResult success) { _subMgrPort.Post(new submgr.Submit( subscribe.Body.Subscriber, DsspActions.ReplaceRequest, _state, null)); }, null )); }
/// <summary> /// Handles incoming set motor messages /// </summary> private IEnumerator <ITask> SetMotorHandler(SetMotor message) { // Requests come too fast, so dump ones that come in too fast. if (RequestPending > 0) { message.ResponsePort.Post(new DefaultUpdateResponseType()); yield break; } RequestPending++; if (message.Body.Motor.ToUpper().Contains("LEFT")) { _state.MotorLeft = message.Body.Speed; } else if (message.Body.Motor.ToUpper().Contains("RIGHT")) { _state.MotorRight = message.Body.Speed; } else { LogError("Motor name not set properly"); } _scribblerComm.SendCommand(new ScribblerCommand((byte)ScribblerHelper.Commands.SET_MOTORS, (byte)_state.MotorRight, (byte)_state.MotorLeft)); yield return(Arbiter.Receive <ScribblerCommand>(false, _scribblerComPort, delegate(ScribblerCommand response) { //if (response.Data[0] != (byte)ScribblerHelper.Commands.SET_MOTORS) // LogError("SetMotor picked up a wrong echo"); //initialize notify list List <string> notify = new List <string>(); notify.Add("MOTORS"); // notify general subscribers subMgrPort.Post(new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest)); // notify selective subscribers submgr.Submit sub = new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest, notify.ToArray()); subMgrPort.Post(sub); //reply to say we are done message.ResponsePort.Post(DefaultUpdateResponseType.Instance); RequestPending--; } )); yield break; }
IEnumerator <ITask> UsbDeviceDetached(ProximityBoardUsbDeviceDetached linkDetached) { Tracer.Trace("TrackRoamerBrickProximityBoardService::UsbDeviceDetached(): " + linkDetached.Description); _submgrPort.Post(new submgr.Submit(new ResetType(), DsspActions.SubmitRequest)); _state.Description = linkDetached.Description; _state.LinkState = "USB Device Detached - closing link to Proximity Board"; _state.IsConnected = false; LogInfo("Closing link to Proximity Board"); yield return (Arbiter.Choice( _pbCommander.Close(), delegate(SuccessResult success) { _state.LinkState = "USB Device Detached - link to Proximity Board closed"; }, delegate(Exception except) { _state.LinkState = "USB Device Detached - Error closing link to Proximity Board"; LogError(except); } )); _state.LinkState = "USB Device Detached - Proximity Board link closed, waiting 5 seconds"; LogInfo(_state.LinkState); _pbCommander = null; SpawnIterator(openDelayMs, StartProximityBoard); yield break; }
public IEnumerator <ITask> SubscribeHandler(drive.Subscribe subscribe) { yield return(Arbiter.Choice( SubscribeHelper(_subMgrPort, subscribe.Body, subscribe.ResponsePort), delegate(SuccessResult success) { _subMgrPort.Post(new submgr.Submit( subscribe.Body.Subscriber, DsspActions.UpdateRequest, _state, null)); }, delegate(Exception ex) { LogError(ex); throw ex; } )); }
public virtual IEnumerator <ITask> SubscribeHandler(Subscribe subscribe) { SubscribeRequest request = subscribe.Body; Console.WriteLine("SubscribeHandler() received Subscription request from Subscriber=" + subscribe.Body.Subscriber + " for: " + request.MessageTypes); submgr.InsertSubscription insert = new submgr.InsertSubscription(request); insert.Body.FilterType = submgr.FilterType.Default; string valid = request.ValidOnly ? "True" : null; List <submgr.QueryType> query = new List <submgr.QueryType>(); if (request.MessageTypes == ChrMessageType.All || request.MessageTypes == ChrMessageType.None) { if (request.ValidOnly) { query.Add(new submgr.QueryType(null, valid)); } } else { // Subscriber supplied a bitmask requesting certain UM6 messages: if ((request.MessageTypes & ChrMessageType.GYRO_PROC) != 0) { query.Add(new submgr.QueryType(Tag_GYRO_PROC, valid)); } if ((request.MessageTypes & ChrMessageType.ACCEL_PROC) != 0) { query.Add(new submgr.QueryType(Tag_ACCEL_PROC, valid)); } if ((request.MessageTypes & ChrMessageType.MAG_PROC) != 0) { query.Add(new submgr.QueryType(Tag_MAG_PROC, valid)); } if ((request.MessageTypes & ChrMessageType.EULER) != 0) { query.Add(new submgr.QueryType(Tag_EULER, valid)); } if ((request.MessageTypes & ChrMessageType.QUAT) != 0) { query.Add(new submgr.QueryType(Tag_QUAT, valid)); } // add more types here to the query } if (query.Count > 0) { insert.Body.QueryList = query.ToArray(); } _subMgrPort.Post(insert); yield return(Arbiter.Choice( insert.ResponsePort, subscribe.ResponsePort.Post, subscribe.ResponsePort.Post )); }
public IEnumerator <ITask> ReplaceHandler(Replace replace) { ProcessConfigurationString(ref _state, replace.Body.configuration); replace.ResponsePort.Post(new dssp.DefaultReplaceResponseType()); _subMgrPort.Post(new submgr.Submit(_state, DsspActions.ReplaceRequest)); _state = replace.Body; yield break; }
/// <summary> /// Handles incoming set motor messages /// </summary> private IEnumerator <ITask> SetMotorHandler(SetMotors message) { if (!_state.Connected) { message.ResponsePort.Post(new Fault()); yield break; } //debug if (message.Body.LeftSpeed < 0 || message.Body.LeftSpeed > 200 || message.Body.RightSpeed < 0 || message.Body.RightSpeed > 200) { LogError("Scribbler SetMotorHandler: target power set incorrect"); } //update state _state.MotorLeft = message.Body.LeftSpeed; _state.MotorRight = message.Body.RightSpeed; //send command ScribblerCommand cmd = new ScribblerCommand((byte)ScribblerHelper.Commands.SET_MOTORS, (byte)_state.MotorRight, (byte)_state.MotorLeft); SendScribblerCommand sendcmd = new SendScribblerCommand(cmd); _scribblerComPort.Post(sendcmd); yield return(Arbiter.Receive <ScribblerResponse>(false, sendcmd.ResponsePort, delegate(ScribblerResponse response) { //initialize notify list List <string> notify = new List <string>(); notify.Add("MOTORS"); // notify general subscribers subMgrPort.Post(new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest)); // notify selective subscribers submgr.Submit sub = new submgr.Submit(_state, dssp.DsspActions.ReplaceRequest, notify.ToArray()); subMgrPort.Post(sub); //reply to say we are done message.ResponsePort.Post(DefaultUpdateResponseType.Instance); } )); yield break; }
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); } ));; }
/// <summary> /// Handle sensor update message from Scribbler /// </summary> public void SensorNotificationHandler(brick.Replace notify) { _state.LeftSensor.RawMeasurement = notify.Body.LightLeft; _state.CenterSensor.RawMeasurement = notify.Body.LightCenter; _state.RightSensor.RawMeasurement = notify.Body.LightRight; _state.LeftSensor.TimeStamp = DateTime.Now; _state.CenterSensor.TimeStamp = DateTime.Now; _state.RightSensor.TimeStamp = DateTime.Now; _state.LeftSensor.NormalizedMeasurement = (double)_state.LeftSensor.RawMeasurement / (double)_state.LeftSensor.RawMeasurementRange; _state.CenterSensor.NormalizedMeasurement = (double)_state.CenterSensor.RawMeasurement / (double)_state.CenterSensor.RawMeasurementRange; _state.RightSensor.NormalizedMeasurement = (double)_state.RightSensor.RawMeasurement / (double)_state.RightSensor.RawMeasurementRange; bool changed = true; if (changed) { //notify subscribers on any bumper pressed or unpressed _subMgrPort.Post(new submgr.Submit(_state, DsspActions.UpdateRequest)); } }
/// <summary> /// Unsubscribe from all prior analog sensor requests /// </summary> private void UnsubscribeAllSensors() { if (_sensorList == null) { return; } foreach (SubscribeResponseType sub in _sensorList.Values) { submgr.SubscriptionManagerPort subMgr = ServiceForwarder <submgr.SubscriptionManagerPort>(sub.SubscriptionManager); subMgr.Post(new submgr.DeleteSubscription(new submgr.DeleteSubscriptionMessage(sub.Subscriber))); } _sensorList.Clear(); }
public IEnumerator <ITask> ReplaceHandler(Replace replace) { int comPort = replace.Body.ComPort; if (comPort > 0) { Srv1Controller.PortName = "COM" + comPort; Srv1Controller = Srv1Controller.Srv1Factory(); _subMgrPort.Post(new submgr.Submit(_state, DsspActions.ReplaceRequest)); _state = replace.Body; } yield break; }
public virtual IEnumerator <ITask> UpdateMotorSpeedHandler(UpdateMotorSpeed update) { //LogInfo("TrackRoamerBrickPowerService:UpdateMotorSpeedHandler()"); coord.ActuatorCoordination coordination = update.GetHeader <coord.ActuatorCoordination>(); if (coordination != null && coordination.Count > 1) { if (!_coordinationList.ContainsKey(coordination.RequestId)) { _coordinationList.Add(coordination.RequestId, new Port <UpdateMotorSpeed>()); Activate( Arbiter.MultipleItemReceive <UpdateMotorSpeed>( false, _coordinationList[coordination.RequestId], coordination.Count, SetCoordinatedMotors)); } _coordinationList[coordination.RequestId].Post(update); yield break; } bool changed = ((update.Body.LeftSpeed >= 0 && _state.MotorSpeed.LeftSpeed != update.Body.LeftSpeed) || (update.Body.RightSpeed >= 0 && _state.MotorSpeed.RightSpeed != update.Body.RightSpeed)); if (update.Body.LeftSpeed != null) { if (update.Body.LeftSpeed >= -1.0 && update.Body.LeftSpeed <= 1.0) { //LogInfo("TrackRoamerBrickPowerService:UpdateMotorSpeedHandler - LeftSpeed=" + update.Body.LeftSpeed + " requested"); _state.MotorSpeed.LeftSpeed = update.Body.LeftSpeed; } else { LogInfo("Error: TrackRoamerBrickPowerService:UpdateMotorSpeedHandler - invalid LeftSpeed=" + update.Body.LeftSpeed + " requested, must be between -1.0 and +1.0"); } } if (update.Body.RightSpeed != null) { if (update.Body.RightSpeed >= -1.0 && update.Body.RightSpeed <= 1.0) { //LogInfo("TrackRoamerBrickPowerService:UpdateMotorSpeedHandler - RightSpeed=" + update.Body.RightSpeed + " requested"); _state.MotorSpeed.RightSpeed = update.Body.RightSpeed; } else { LogInfo("Error: TrackRoamerBrickPowerService:UpdateMotorSpeedHandler - invalid RightSpeed=" + update.Body.RightSpeed + " requested, must be between -1.0 and +1.0"); } } // If we are connected, send the speed to the robot wheels controller: if (ensureHardwareController()) { double?leftSpeed = _state.MotorSpeed.LeftSpeed; double?rightSpeed = _state.MotorSpeed.RightSpeed; Tracer.Trace("IP: TrackRoamerBrickPowerService:UpdateMotorSpeedHandler - speed L: " + leftSpeed + " R: " + rightSpeed); // it will take time for upper layers to react on whiskers. We want to have some protection here, to avoid damage. // Note: while moving forward the speeds are negative. // cannot move forward if whiskers are pressed; replace it with backwards movement at half speed though: if (leftSpeed < 0 && _state.Whiskers.FrontWhiskerLeft.GetValueOrDefault()) { Tracer.Trace("Warning: TrackRoamerBrickPowerService:UpdateMotorSpeedHandler - left whisker pressed, speed " + leftSpeed + " reversed"); leftSpeed = -leftSpeed / 2; } if (rightSpeed < 0 && _state.Whiskers.FrontWhiskerRight.GetValueOrDefault()) { Tracer.Trace("Warning: TrackRoamerBrickPowerService:UpdateMotorSpeedHandler - right whisker pressed, speed " + rightSpeed + " reversed"); rightSpeed = -rightSpeed / 2; } _brickConnection.SetSpeed(leftSpeed, rightSpeed); } // Send Notifications to subscribers if (changed) { _subMgrPort.Post(new submgr.Submit(_state.MotorSpeed, DsspActions.UpdateRequest)); } _state.TimeStamp = _state.MotorSpeed.Timestamp = DateTime.Now; update.ResponsePort.Post(DefaultUpdateResponseType.Instance); yield break; }
/// <summary> /// convert sonar sweep into laser-like 180 degrees data /// </summary> /// <param name="update"></param> void trpbUpdateSonarNotification(proxibrick.UpdateSonarData update) { //LogInfo("TrackRoamerUsrfService: trpbUpdateNotification()"); //Tracer.Trace("TrackRoamerUsrfService: trpbUpdateNotification()"); try { proxibrick.SonarDataDssSerializable p = update.Body; int packetLength = p.RangeMeters.Length; // must be 26 packets, each covering 7 degrees; if (packetLength != 26) { Tracer.Error("TrackRoamerUsrfService::trpbUpdateNotification() not a standard measurement, angles.Count=" + packetLength + " (expected 26) -- ignored"); return; } int[] intRanges = new int[packetLength]; for (int i = 0; i < packetLength; i++) { // range = (ushort)(i * 40); ushort range = (ushort)Math.Round(p.RangeMeters[i] * 1000.0d); if (range > 0x1FF7) { range = 0x2000; // limit to 8192 mm; actual range about 4 meters } intRanges[i] = (int)range; } if (doWeeding) { if (intRanges[0] < (intRanges[1] + intRanges[2]) / 4) { intRanges[0] = (intRanges[1] + intRanges[2]) / 2; } if (intRanges[packetLength - 1] < (intRanges[packetLength - 2] + intRanges[packetLength - 3]) / 4) { intRanges[packetLength - 1] = (intRanges[packetLength - 2] + intRanges[packetLength - 3]) / 2; } for (int i = 1; i < packetLength - 1; i++) { if (intRanges[i] < (intRanges[i - 1] + intRanges[i + 1]) * 3 / 8) { intRanges[i] = (intRanges[i - 1] + intRanges[i + 1]) / 2; } } } int angularRange = 180; int angularResolution = 1; int mesLength = angularRange + 1; // 181 int[] lsdRanges = new int[mesLength]; // millimeters, with 1 degree resolution int step = (int)Math.Round((double)mesLength / (double)packetLength); // typically round(6.96) = 7 // if we smooth the measurements, Decide() has better chance of sorting the values right. It does not like 7 degrees steps. // we need these for exponential moving average: double emaPeriod = 4.0d; double emaMultiplier = 2.0d / (1.0d + emaPeriod); double?emaPrev = null; int iRange = 0; for (int i = 0; i < mesLength; i++) // 0...181 { int angleIndex = Math.Min(i / step, packetLength - 1); iRange = intRanges[angleIndex]; if (doAveraging) { // calculate exponential moving average - smooth the curve a bit: double?ema = !emaPrev.HasGoodValue() ? iRange : ((iRange - emaPrev) * emaMultiplier + emaPrev); emaPrev = ema; iRange = (int)Math.Round((double)ema); } //Tracer.Trace("&&&&&&&&&&&&&&&&&&&&&&&&&&&& i=" + i + " range=" + range + " ema=" + iRange); lsdRanges[i] = iRange; // 5000; // milimeters } _state.AngularRange = angularRange; _state.AngularResolution = angularResolution; _state.DistanceMeasurements = lsdRanges; _state.Units = sicklrf.Units.Millimeters; _state.TimeStamp = p.TimeStamp; _state.LinkState = "Measurement received"; // // Inform subscribed services that the state has changed. // _submgrPort.Post(new submgr.Submit(_state, DsspActions.ReplaceRequest)); } catch (Exception exc) { LogError(exc); } }