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