void toWGS84(const IMC::EstimatedState& estate, double& lat, double& lon) { // Use reference height. float hae = estate.height; toWGS84(estate, lat, lon, hae); }
GlobalReference& GlobalReference::setCurrentPosition(const State& state, double new_latitude, double new_longitude) { State::ConstPositionType position = state.getPosition(); // set reference to new latitude/longitude first (intermediate reference) setPosition(new_latitude, new_longitude, true); // convert current position back to WGS84 using the new reference position // and reset the reference position so that current position in x/y coordinates remains the same // This will work unless the radii at the origin and the x/y position of the robot differ too much toWGS84(-position.x(), -position.y(), new_latitude, new_longitude); setPosition(new_latitude, new_longitude); return *this; }
GlobalReference& GlobalReference::setCurrentHeading(const State& state, double new_heading) { // get current yaw angle double current_yaw = state.getYaw(); State::ConstPositionType position = state.getPosition(); // get current position in WGS84 double current_latitude, current_longitude; if (hasPosition()) { toWGS84(position.x(), position.y(), current_latitude, current_longitude); } // set the new reference heading setHeading(new_heading - (-current_yaw)); // set the new reference position so that current position in WGS84 coordinates remains the same as before if (hasPosition()) { setCurrentPosition(state, current_latitude, current_longitude); } return *this; }