39 GlobalReference::GlobalReference()
41 parameters().add(
"reference_latitude", reference_latitude_ = std::numeric_limits<double>::quiet_NaN());
42 parameters().add(
"reference_longitude", reference_longitude_ = std::numeric_limits<double>::quiet_NaN());
43 parameters().add(
"reference_altitude", reference_altitude_ = std::numeric_limits<double>::quiet_NaN());
44 parameters().add(
"reference_heading", heading_.value = std::numeric_limits<double>::quiet_NaN());
56 void GlobalReference::reset()
63 position_.latitude = reference_latitude_ * M_PI/180.0;
64 position_.longitude = reference_longitude_ * M_PI/180.0;
65 position_.altitude = reference_altitude_;
66 heading_.value = reference_heading_;
75 void GlobalReference::updated(
bool intermediate) {
78 radius_ =
Radius(position_.latitude);
83 sincos(heading_.value, &heading_.sin, &heading_.cos);
88 for(std::list<UpdateCallback>::iterator cb = update_callbacks_.begin(); cb != update_callbacks_.end(); ++cb)
95 sincos(heading, &
sin, &
cos);
101 sincos(0.5 *
value, &sin_2, &cos_2);
107 static const double equatorial_radius = 6378137.0;
108 static const double flattening = 1.0/298.257223563;
109 static const double excentrity2 = 2*flattening - flattening*flattening;
111 double temp = 1.0 / (1.0 - excentrity2 *
sin(latitude) *
sin(latitude));
112 double prime_vertical_radius = equatorial_radius * sqrt(temp);
113 north = prime_vertical_radius * (1 - excentrity2) * temp;
114 east = prime_vertical_radius *
cos(latitude);
164 if (!intermediate)
ROS_INFO(
"Set new reference position to %f deg N / %f deg E", this->
position().latitude * 180.0/M_PI, this->
position().longitude * 180.0/M_PI);
171 if (!intermediate)
ROS_INFO(
"Set new reference heading to %.1f degress", this->
heading() * 180.0 / M_PI);
178 if (!intermediate)
ROS_INFO(
"Set new reference altitude to %.2f m", this->
position().altitude);
192 toWGS84(-position.x(), -position.y(), new_latitude, new_longitude);
200 double current_yaw = state.
getYaw();
204 double current_latitude, current_longitude;
206 toWGS84(position.x(), position.y(), current_latitude, current_longitude);
229 geopose.orientation.w = orientation.w();
230 geopose.orientation.x = orientation.x();
231 geopose.orientation.y = orientation.y();
232 geopose.orientation.z = orientation.z();
249 transform.header.stamp = stamp;
250 transform.header.frame_id = world_frame;
251 transform.child_frame_id = nav_frame;
257 transform.transform.translation.x = north * reference_heading.
cos + east * reference_heading.
sin;
258 transform.transform.translation.y = north * reference_heading.
sin - east * reference_heading.
cos;
260 double heading_diff =
heading().
value - reference_heading;
261 transform.transform.rotation.w =
cos(heading_diff / 2.);
262 transform.transform.rotation.x = 0.0;
263 transform.transform.rotation.y = 0.0;
264 transform.transform.rotation.z = -
sin(heading_diff / 2.);
double reference_latitude_
bool getWorldToNavTransform(geometry_msgs::TransformStamped &transform, const std::string &world_frame, const std::string &nav_frame, const ros::Time &stamp=ros::Time()) const
const Heading & heading() const
Measurement_< HeadingModel > Heading
void fromWGS84(double latitude, double longitude, double &x, double &y)
void toWGS84(double x, double y, double &latitude, double &longitude)
std::list< UpdateCallback > update_callbacks_
void toNorthEast(double x, double y, double &north, double &east)
GlobalReference & setCurrentAltitude(const State &state, double altitude)
Eigen::Quaternion< ScalarType > Quaternion
void addUpdateCallback(const UpdateCallback &)
GlobalReference & setCurrentHeading(const State &state, double heading)
GlobalReference & setAltitude(double altitude, bool intermediate=false)
GlobalReference & setCurrentPosition(const State &state, double latitude, double longitude)
double reference_altitude_
void getGeoPose(geographic_msgs::GeoPose &geopose) const
const Position & position() const
double reference_longitude_
double reference_heading_
boost::function< void()> UpdateCallback
GlobalReference & setPosition(double latitude, double longitude, bool intermediate=false)
void fromNorthEast(double north, double east, double &x, double &y)
VectorBlock< const Vector, 3 > ConstPositionType
Quaternion quaternion() const
void updated(bool intermediate=false)
virtual ConstPositionType getPosition() const
GlobalReference & setHeading(double heading, bool intermediate=false)