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);
}
}
