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