Пример #1
0
        /// <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);
        }
Пример #2
0
        /// <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);
        }