public RobotPoseBase() { geoPosition = new GeoPosition(0.0d, 0.0d); direction = new Direction(); H = 0.0d; }
/// <summary> /// this constructor is used only for reading mission files /// </summary> /// <param name="lw"></param> internal Trackpoint(Locationwp lw) : base(lw.lat, lw.lng, lw.alt) { trackpointState = TrackpointState.None; number = lw.number; id = (MAV_CMD)Enum.Parse(typeof(MAV_CMD), lw.id.ToString()); // command id isHome = lw.ishome != 0; coordinateFrameOption = lw.options == 1 ? CoordinateFrameOption.MAV_FRAME_GLOBAL_RELATIVE_ALT : CoordinateFrameOption.MAV_FRAME_GLOBAL; // alt is in meters, can be above the ground (AGL) or above mean sea level (MSL), depending on coordinateFrameOption geoPosition = new GeoPosition(lw.lng, lw.lat, lw.alt); // p1-p4 are just float point numbers that can be added to waypoints and be interpreted by behaviors. We pass them all directly. p1 = lw.p1; p2 = lw.p2; p3 = lw.p3; p4 = lw.p4; }
/// <summary> /// given current robot location, goal bearing and distance, compute and fill goalGeoPosition /// </summary> public void computeGoalGeoPosition(IRobotPose currentPose) { var pos = new GeoPosition(currentPose.geoPosition); pos.translateToDirection(new Direction() { heading = goalBearingDegrees }, new Distance(goalDistanceMeters.Value)); goalGeoPosition = pos; }