void wheelPositionUpdate(object sender, TimestampedEventArgs <IRobotTwoWheelStatus> e)
{
curTS = e.TimeStamp;
double curLeftPos = e.Message.IntegratedLeftWheelPosition;
double curRightPos = e.Message.IntegratedRightWheelPosition;
double curLeftSpeed = e.Message.LeftWheelSpeed;
double curRightSpeed = e.Message.RightWheelSpeed;
double ts = e.TimeStamp;
if (!gotFirstUpdate)
{
prevIntegratedLeft = curLeftPos;
prevIntegratedRight = curRightPos;
prevTS = curTS;
gotFirstUpdate = true;
return;
}
//get the current thing
double diffL = curLeftPos - prevIntegratedLeft;
double diffR = curRightPos - prevIntegratedRight;
double deltaT = curTS - prevTS;
prevIntegratedLeft = curLeftPos;
prevIntegratedRight = curRightPos;
prevTS = curTS;
//from lavalle's book ; http://planning.cs.uiuc.edu/node659.html
//calculate the derivatives in measurements we have
double xdot = ((diffL + diffR) / 2.0) * Math.Cos(state.yaw);
double ydot = ((diffL + diffR) / 2.0) * Math.Sin(state.yaw);
double headingDot = (diffR - diffL) / robot.TrackWidth;
//update the state vector
state.x += xdot;
state.y += ydot;
state.yaw += headingDot;
state.yaw = Pose.pi2pi(state.yaw);
state.timestamp = ts;
//update the wheel measurements
if (OdometryUpdate != null)
{
OdometryUpdate(this, new TimestampedEventArgs <RobotPose> (ts, CurrentState));
}
if (EncoderUpdate != null)
{
EncoderUpdate(this, new TimestampedEventArgs <OdometryData> (ts, new OdometryData(curLeftPos, curRightPos, diffL, diffR, curLeftSpeed, curRightSpeed, deltaT)));
}
}
public void WhenLoadingFromEvent_ThenRootChangesState()
{
var root = new TestRoot();
var events = new TimestampedEventArgs[] { new TestPublished {
Version = 5
} };
root.Load(events);
Assert.Equal(5, root.LatestVersion);
Assert.False(root.GetChanges().Any());
// This should be no-op now.
root.AcceptChanges();
Assert.False(root.GetChanges().Any());
}
void wheelPositionUpdate(object sender, TimestampedEventArgs<IRobotTwoWheelStatus> e)
{
curTS = e.TimeStamp;
double curLeftPos = e.Message.IntegratedLeftWheelPosition;
double curRightPos = e.Message.IntegratedRightWheelPosition;
double curLeftSpeed = e.Message.LeftWheelSpeed;
double curRightSpeed = e.Message.RightWheelSpeed;
double ts = e.TimeStamp;
if (!gotFirstUpdate)
{
prevIntegratedLeft = curLeftPos;
prevIntegratedRight = curRightPos;
prevTS = curTS;
gotFirstUpdate = true;
return;
}
//get the current thing
double diffL = curLeftPos - prevIntegratedLeft;
double diffR = curRightPos - prevIntegratedRight;
double deltaT = curTS - prevTS;
prevIntegratedLeft = curLeftPos;
prevIntegratedRight = curRightPos;
prevTS = curTS;
//from lavalle's book ; http://planning.cs.uiuc.edu/node659.html
//calculate the derivatives in measurements we have
double xdot = ((diffL + diffR) / 2.0) * Math.Cos(state.yaw);
double ydot = ((diffL + diffR) / 2.0) * Math.Sin(state.yaw);
double headingDot = (diffR - diffL) / robot.TrackWidth;
//update the state vector
state.x += xdot;
state.y += ydot;
state.yaw += headingDot;
state.yaw = Pose.pi2pi(state.yaw);
state.timestamp = ts;
//update the wheel measurements
if (OdometryUpdate != null) OdometryUpdate(this, new TimestampedEventArgs<RobotPose> (ts,CurrentState));
if (EncoderUpdate != null) EncoderUpdate(this, new TimestampedEventArgs<OdometryData> (ts, new OdometryData (curLeftPos,curRightPos, diffL,diffR, curLeftSpeed, curRightSpeed, deltaT)));
}
/// <summary>
/// Publishes the specified event to the bus so that all subscribers are notified.
/// </summary>
/// <param name="sender">The sender of the event.</param>
/// <param name="args">The event payload.</param>
public virtual void Publish(AggregateRoot <TId> sender, TimestampedEventArgs args)
{
Guard.NotNull(() => sender, sender);
Guard.NotNull(() => args, args);
var compatibleHandlers = this.eventHandlers.Where(h => h.EventType.IsAssignableFrom(args.GetType())).ToList();
dynamic dynamicEvent = args;
// By making this dynamic, we allow event handlers to subscribe to base classes
foreach (dynamic handler in compatibleHandlers.Where(h => !h.IsAsync).AsParallel())
{
handler.Handle(sender.Id, dynamicEvent);
}
// Run background handlers through the async runner.
foreach (dynamic handler in compatibleHandlers.Where(h => h.IsAsync).AsParallel())
{
asyncActionRunner(() => handler.Handle(sender.Id, args));
}
}
private void ParsePVTGeodetic(byte idRev, byte[] msg, double ts, byte septID)
{
GPSPositionData posData = new GPSPositionData();
BinaryReader br = new BinaryReader(new MemoryStream(msg));
br.ReadBytes(8); //knock off the first 8 bytes
double TOW = br.ReadUInt32() * .001; //seconds
double WNc = br.ReadUInt16(); //week number
byte mode = br.ReadByte();
byte error = br.ReadByte();
double latitude = br.ReadDouble(); //in radians!
double longitude = br.ReadDouble();
double height = br.ReadDouble();
double undulation = br.ReadDouble();
double velN = br.ReadDouble();
double velE = br.ReadDouble();
double velU = br.ReadDouble();
posData.position.alt = height;
posData.position.lat = latitude * 180.0 / Math.PI;
posData.position.lon = longitude * 180.0 / Math.PI;
posData.timeOfFix = TOW;
posData.timestamp = ts;
if (PositionMeasurementReceived != null)
{
TimestampedEventArgs <GPSPositionData> GPSData = new TimestampedEventArgs <GPSPositionData>(ts, posData);
if (septID == 0)
{
GPSData.Message.DataType = "frontGPS";
}
else if (septID == 1)
{
GPSData.Message.DataType = "rearGPS";
}
PositionMeasurementReceived(this, GPSData);
}
}
/// <summary>
/// Does nothing.
/// </summary>
public void Publish(AggregateRoot <TId> sender, TimestampedEventArgs args)
{
}
void segway_WheelPositionUpdate(object sender, TimestampedEventArgs<Magic.Common.Robots.IRobotTwoWheelStatus> e)
{
timeStamp = e.TimeStamp;
}
void segway_WheelPositionUpdate(object sender, TimestampedEventArgs <Magic.Common.Robots.IRobotTwoWheelStatus> e)
{
timeStamp = e.TimeStamp;
}
public StoredEvent(AggregateRoot <TId> sender, TimestampedEventArgs args)
{
this.AggregateRoot = sender;
this.EventArgs = args;
}
public void Save(AggregateRoot <TId> sender, TimestampedEventArgs args)
{
this.events.Add(new StoredEvent(sender, args));
}
private void ParsePVTGeodetic(byte idRev, byte[] msg, double ts, byte septID)
{
GPSPositionData posData = new GPSPositionData();
BinaryReader br = new BinaryReader(new MemoryStream(msg));
br.ReadBytes(8); //knock off the first 8 bytes
double TOW = br.ReadUInt32() * .001; //seconds
double WNc = br.ReadUInt16(); //week number
byte mode = br.ReadByte();
byte error = br.ReadByte();
double latitude = br.ReadDouble(); //in radians!
double longitude = br.ReadDouble();
double height = br.ReadDouble();
double undulation = br.ReadDouble();
double velN = br.ReadDouble();
double velE = br.ReadDouble();
double velU = br.ReadDouble();
posData.position.alt = height;
posData.position.lat = latitude * 180.0 / Math.PI;
posData.position.lon = longitude * 180.0 / Math.PI;
posData.timeOfFix = TOW;
posData.timestamp = ts;
if (PositionMeasurementReceived != null)
{
TimestampedEventArgs<GPSPositionData> GPSData = new TimestampedEventArgs<GPSPositionData>(ts, posData);
if (septID == 0)
GPSData.Message.DataType = "frontGPS";
else if (septID == 1)
GPSData.Message.DataType = "rearGPS";
PositionMeasurementReceived(this, GPSData);
}
}
