/// <summary>
/// Convert UGV State to system state
/// </summary>
/// <returns></returns>
public SystemState ToSystemState(UGV ugv)
{
SystemState state = new SystemState();
state.Lat = this.Latitude;
state.Long = this.Longitude;
state.Bearing = this.Heading;
WayPoint waypoint;
var tempTargets = ugv.VisionTargets.ToArray();
if (tempTargets.Length > 0 &&
(ugv.State == UGV.DriveState.SearchTarget || ugv.State == UGV.DriveState.LockTarget))
{
state.TargetX = tempTargets[0].Lat;
state.TargetY = tempTargets[0].Long;
}
else if (ugv.Waypoints.TryPeek(out waypoint))
{
state.TargetX = waypoint.Lat;
state.TargetY = waypoint.Long;
}
else
{
state.TargetX = this.Latitude;
state.TargetY = this.Longitude;
}
state.Velocity = 0;
return(state);
}
/// <summary>
/// Capture UGV state
/// </summary>
/// <param name="ugv"></param>
/// <returns></returns>
public static UGVState Capture(UGV ugv)
{
UGVState state = new UGVState();
state.Latitude = ugv.Latitude;
state.Longitude = ugv.Longitude;
state.Pitch = ugv.Pitch;
state.Roll = ugv.Roll;
state.Heading = ugv.Heading;
state.FrontWheelOutput = ugv.FinalFrontWheel;
state.RearWheelOutput = ugv.FinalRearWheel;
state.SteerOutput = ugv.FinalSteering;
state.WayPointBearing = ugv.NextWaypointBearing;
state.WayPointDistance = ugv.NextWaypointDistance;
state.DriveMode = (byte)ugv.Settings.DriveMode;
state.TargetLocation = ugv.TargetLockedLocation;
return(state);
}