#include <navsat_transform.h>
Public Member Functions | |
NavSatTransform (ros::NodeHandle nh, ros::NodeHandle nh_priv) | |
Constructor. More... | |
~NavSatTransform () | |
Destructor. More... | |
Private Member Functions | |
nav_msgs::Odometry | cartesianToMap (const tf2::Transform &cartesian_pose) const |
Transforms the passed in pose from utm to map frame. More... | |
void | computeTransform () |
Computes the transform from the UTM frame to the odom frame. More... | |
bool | datumCallback (robot_localization::SetDatum::Request &request, robot_localization::SetDatum::Response &) |
Callback for the datum service. More... | |
bool | fromLLCallback (robot_localization::FromLL::Request &request, robot_localization::FromLL::Response &response) |
Callback for the from Lat Long service. More... | |
void | getRobotOriginCartesianPose (const tf2::Transform &gps_cartesian_pose, tf2::Transform &robot_cartesian_pose, const ros::Time &transform_time) |
Given the pose of the navsat sensor in the cartesian frame, removes the offset from the vehicle's centroid and returns the cartesian-frame pose of said centroid. More... | |
void | getRobotOriginWorldPose (const tf2::Transform &gps_odom_pose, tf2::Transform &robot_odom_pose, const ros::Time &transform_time) |
Given the pose of the navsat sensor in the world frame, removes the offset from the vehicle's centroid and returns the world-frame pose of said centroid. More... | |
void | gpsFixCallback (const sensor_msgs::NavSatFixConstPtr &msg) |
Callback for the GPS fix data. More... | |
void | imuCallback (const sensor_msgs::ImuConstPtr &msg) |
Callback for the IMU data. More... | |
void | mapToLL (const tf2::Vector3 &point, double &latitude, double &longitude, double &altitude) const |
Transforms the passed in point from map frame to lat/long. More... | |
void | odomCallback (const nav_msgs::OdometryConstPtr &msg) |
Callback for the odom data. More... | |
void | periodicUpdate (const ros::TimerEvent &event) |
callback function which is called for periodic updates More... | |
bool | prepareFilteredGps (sensor_msgs::NavSatFix &filtered_gps) |
Converts the odometry data back to GPS and broadcasts it. More... | |
bool | prepareGpsOdometry (nav_msgs::Odometry &gps_odom) |
Prepares the GPS odometry message before sending. More... | |
void | setTransformGps (const sensor_msgs::NavSatFixConstPtr &msg) |
Used for setting the GPS data that will be used to compute the transform. More... | |
void | setTransformOdometry (const nav_msgs::OdometryConstPtr &msg) |
Used for setting the odometry data that will be used to compute the transform. More... | |
bool | setUTMZoneCallback (robot_localization::SetUTMZone::Request &request, robot_localization::SetUTMZone::Response &response) |
Callback for the UTM zone service. More... | |
bool | toLLCallback (robot_localization::ToLL::Request &request, robot_localization::ToLL::Response &response) |
Callback for the to Lat Long service. More... | |
Private Attributes | |
std::string | base_link_frame_id_ |
Frame ID of the robot's body frame. More... | |
bool | broadcast_cartesian_transform_ |
Whether or not we broadcast the cartesian transform. More... | |
bool | broadcast_cartesian_transform_as_parent_frame_ |
Whether to broadcast the cartesian transform as parent frame, default as child. More... | |
tf2_ros::StaticTransformBroadcaster | cartesian_broadcaster_ |
Used for publishing the static world_frame->cartesian transform. More... | |
std::string | cartesian_frame_id_ |
The cartesian frame ID, default as 'local_enu' if using Local Cartesian or 'utm' if using the UTM coordinates as our cartesian. More... | |
tf2::Transform | cartesian_world_trans_inverse_ |
Holds the odom->UTM transform for filtered GPS broadcast. More... | |
tf2::Transform | cartesian_world_transform_ |
Holds the Cartesian->odom transform. More... | |
ros::ServiceServer | datum_srv_ |
Service for set datum. More... | |
ros::Publisher | filtered_gps_pub_ |
Publiser for filtered gps data. More... | |
bool | force_user_utm_ |
Whether we want to force the user's UTM zone and not rely on current GPS data for determining it. More... | |
ros::ServiceServer | from_ll_srv_ |
Service for from Lat Long. More... | |
std::string | gps_frame_id_ |
The frame_id of the GPS message (specifies mounting location) More... | |
GeographicLib::LocalCartesian | gps_local_cartesian_ |
Local Cartesian projection around gps origin. More... | |
ros::Publisher | gps_odom_pub_ |
Publisher for gps data. More... | |
ros::Subscriber | gps_sub_ |
GPS subscriber. More... | |
ros::Time | gps_update_time_ |
Timestamp of the latest good GPS message. More... | |
bool | gps_updated_ |
Whether or not we have new GPS data. More... | |
bool | has_transform_gps_ |
Whether or not the GPS fix is usable. More... | |
bool | has_transform_imu_ |
Signifies that we have received a usable IMU message. More... | |
bool | has_transform_odom_ |
Signifies that we have received a usable odometry message. More... | |
ros::Subscriber | imu_sub_ |
Subscribes to imu topic. More... | |
Eigen::MatrixXd | latest_cartesian_covariance_ |
Covariance for most recent GPS/UTM/LocalCartesian data. More... | |
tf2::Transform | latest_cartesian_pose_ |
Latest GPS data, stored as Cartesian coords. More... | |
Eigen::MatrixXd | latest_odom_covariance_ |
Covariance for most recent odometry data. More... | |
tf2::Transform | latest_world_pose_ |
Latest odometry pose data. More... | |
double | magnetic_declination_ |
Parameter that specifies the magnetic declination for the robot's environment. More... | |
bool | northp_ |
hemisphere (true means north, false means south) More... | |
ros::Subscriber | odom_sub_ |
Odometry subscriber. More... | |
ros::Time | odom_update_time_ |
Timestamp of the latest good odometry message. More... | |
bool | odom_updated_ |
Whether or not we have new odometry data. More... | |
ros::Timer | periodicUpdateTimer_ |
timer calling periodicUpdate More... | |
bool | publish_gps_ |
Whether or not we publish filtered GPS messages. More... | |
ros::ServiceServer | set_utm_zone_srv_ |
Service for set UTM zone. More... | |
tf2_ros::Buffer | tf_buffer_ |
Transform buffer for managing coordinate transforms. More... | |
tf2_ros::TransformListener | tf_listener_ |
Transform listener for receiving transforms. More... | |
bool | tf_silent_failure_ |
When true, do not print warnings for tf lookup failures. More... | |
ros::ServiceServer | to_ll_srv_ |
Service for to Lat Long. More... | |
tf2::Transform | transform_cartesian_pose_ |
Holds the cartesian (UTM or local ENU) pose that is used to compute the transform. More... | |
bool | transform_good_ |
Whether or not we've computed a good heading. More... | |
tf2::Quaternion | transform_orientation_ |
Latest IMU orientation. More... | |
ros::Duration | transform_timeout_ |
Parameter that specifies the how long we wait for a transform to become available. More... | |
tf2::Transform | transform_world_pose_ |
Latest IMU orientation. More... | |
bool | use_local_cartesian_ |
Whether we use a Local Cartesian (tangent plane ENU) or the UTM coordinates as our cartesian. More... | |
bool | use_manual_datum_ |
Whether we get our datum from the first GPS message or from the set_datum service/parameter. More... | |
bool | use_odometry_yaw_ |
Whether we get the transform's yaw from the odometry or IMU source. More... | |
double | utm_meridian_convergence_ |
UTM's meridian convergence. More... | |
int | utm_zone_ |
the UTM zone (zero means UPS) More... | |
std::string | world_frame_id_ |
Frame ID of the GPS odometry output. More... | |
double | yaw_offset_ |
IMU's yaw offset. More... | |
bool | zero_altitude_ |
Whether or not to report 0 altitude. More... | |
Definition at line 64 of file navsat_transform.h.
RobotLocalization::NavSatTransform::NavSatTransform | ( | ros::NodeHandle | nh, |
ros::NodeHandle | nh_priv | ||
) |
Constructor.
Definition at line 46 of file navsat_transform.cpp.
RobotLocalization::NavSatTransform::~NavSatTransform | ( | ) |
Destructor.
Definition at line 200 of file navsat_transform.cpp.
|
private |
Transforms the passed in pose from utm to map frame.
[in] | cartesian_pose | the pose in cartesian frame to use to transform |
Definition at line 465 of file navsat_transform.cpp.
|
private |
Computes the transform from the UTM frame to the odom frame.
Definition at line 236 of file navsat_transform.cpp.
|
private |
Callback for the datum service.
Definition at line 342 of file navsat_transform.cpp.
|
private |
Callback for the from Lat Long service.
Definition at line 401 of file navsat_transform.cpp.
|
private |
Given the pose of the navsat sensor in the cartesian frame, removes the offset from the vehicle's centroid and returns the cartesian-frame pose of said centroid.
Definition at line 519 of file navsat_transform.cpp.
|
private |
Given the pose of the navsat sensor in the world frame, removes the offset from the vehicle's centroid and returns the world-frame pose of said centroid.
Definition at line 570 of file navsat_transform.cpp.
|
private |
Callback for the GPS fix data.
[in] | msg | The NavSatFix message to process |
Definition at line 618 of file navsat_transform.cpp.
|
private |
Callback for the IMU data.
[in] | msg | The IMU message to process |
Definition at line 684 of file navsat_transform.cpp.
|
private |
Transforms the passed in point from map frame to lat/long.
[in] | point | the point in map frame to use to transform |
Definition at line 485 of file navsat_transform.cpp.
|
private |
Callback for the odom data.
[in] | msg | The odometry message to process |
Definition at line 740 of file navsat_transform.cpp.
|
private |
callback function which is called for periodic updates
Definition at line 205 of file navsat_transform.cpp.
|
private |
Converts the odometry data back to GPS and broadcasts it.
[out] | filtered_gps | The NavSatFix message to prepare |
Definition at line 765 of file navsat_transform.cpp.
|
private |
Prepares the GPS odometry message before sending.
[out] | gps_odom | The odometry message to prepare |
Definition at line 813 of file navsat_transform.cpp.
|
private |
Used for setting the GPS data that will be used to compute the transform.
[in] | msg | The NavSatFix message to use in the transform |
Definition at line 867 of file navsat_transform.cpp.
|
private |
Used for setting the odometry data that will be used to compute the transform.
[in] | msg | The odometry message to use in the transform |
Definition at line 910 of file navsat_transform.cpp.
|
private |
Callback for the UTM zone service.
Definition at line 448 of file navsat_transform.cpp.
|
private |
Callback for the to Lat Long service.
Definition at line 386 of file navsat_transform.cpp.
|
private |
Frame ID of the robot's body frame.
This is needed for obtaining transforms from the robot's body frame to the frames of sensors (IMU and GPS)
Definition at line 254 of file navsat_transform.h.
|
private |
Whether or not we broadcast the cartesian transform.
Definition at line 162 of file navsat_transform.h.
|
private |
Whether to broadcast the cartesian transform as parent frame, default as child.
Definition at line 166 of file navsat_transform.h.
|
private |
Used for publishing the static world_frame->cartesian transform.
Definition at line 381 of file navsat_transform.h.
|
private |
The cartesian frame ID, default as 'local_enu' if using Local Cartesian or 'utm' if using the UTM coordinates as our cartesian.
Definition at line 259 of file navsat_transform.h.
|
private |
Holds the odom->UTM transform for filtered GPS broadcast.
Definition at line 333 of file navsat_transform.h.
|
private |
Holds the Cartesian->odom transform.
Definition at line 329 of file navsat_transform.h.
|
private |
Service for set datum.
Definition at line 357 of file navsat_transform.h.
|
private |
Publiser for filtered gps data.
Definition at line 337 of file navsat_transform.h.
|
private |
Whether we want to force the user's UTM zone and not rely on current GPS data for determining it.
Definition at line 215 of file navsat_transform.h.
|
private |
Service for from Lat Long.
Definition at line 365 of file navsat_transform.h.
|
private |
The frame_id of the GPS message (specifies mounting location)
Definition at line 263 of file navsat_transform.h.
|
private |
Local Cartesian projection around gps origin.
Definition at line 223 of file navsat_transform.h.
|
private |
Publisher for gps data.
Definition at line 353 of file navsat_transform.h.
|
private |
GPS subscriber.
Definition at line 341 of file navsat_transform.h.
|
private |
Timestamp of the latest good GPS message.
We assign this value to the timestamp of the odometry message that we output
Definition at line 291 of file navsat_transform.h.
|
private |
Whether or not we have new GPS data.
We only want to compute and broadcast our transformed GPS data if it's new. This variable keeps track of that.
Definition at line 172 of file navsat_transform.h.
|
private |
Whether or not the GPS fix is usable.
Definition at line 176 of file navsat_transform.h.
|
private |
Signifies that we have received a usable IMU message.
Definition at line 180 of file navsat_transform.h.
|
private |
Signifies that we have received a usable odometry message.
Definition at line 184 of file navsat_transform.h.
|
private |
Subscribes to imu topic.
Definition at line 345 of file navsat_transform.h.
|
private |
Covariance for most recent GPS/UTM/LocalCartesian data.
Definition at line 285 of file navsat_transform.h.
|
private |
Latest GPS data, stored as Cartesian coords.
Definition at line 313 of file navsat_transform.h.
|
private |
Covariance for most recent odometry data.
Definition at line 281 of file navsat_transform.h.
|
private |
Latest odometry pose data.
Definition at line 317 of file navsat_transform.h.
|
private |
Parameter that specifies the magnetic declination for the robot's environment.
Definition at line 233 of file navsat_transform.h.
|
private |
hemisphere (true means north, false means south)
Definition at line 271 of file navsat_transform.h.
|
private |
Odometry subscriber.
Definition at line 349 of file navsat_transform.h.
|
private |
Timestamp of the latest good odometry message.
We assign this value to the timestamp of the odometry message that we output
Definition at line 297 of file navsat_transform.h.
|
private |
Whether or not we have new odometry data.
If we're creating filtered GPS messages, then we only want to broadcast them when new odometry data arrives.
Definition at line 190 of file navsat_transform.h.
|
private |
timer calling periodicUpdate
Definition at line 305 of file navsat_transform.h.
|
private |
Whether or not we publish filtered GPS messages.
Definition at line 194 of file navsat_transform.h.
|
private |
Service for set UTM zone.
Definition at line 369 of file navsat_transform.h.
|
private |
Transform buffer for managing coordinate transforms.
Definition at line 373 of file navsat_transform.h.
|
private |
Transform listener for receiving transforms.
Definition at line 377 of file navsat_transform.h.
|
private |
When true, do not print warnings for tf lookup failures.
Definition at line 219 of file navsat_transform.h.
|
private |
Service for to Lat Long.
Definition at line 361 of file navsat_transform.h.
|
private |
Holds the cartesian (UTM or local ENU) pose that is used to compute the transform.
Definition at line 321 of file navsat_transform.h.
|
private |
Whether or not we've computed a good heading.
Definition at line 198 of file navsat_transform.h.
|
private |
Latest IMU orientation.
Definition at line 309 of file navsat_transform.h.
|
private |
Parameter that specifies the how long we wait for a transform to become available.
Definition at line 301 of file navsat_transform.h.
|
private |
Latest IMU orientation.
Definition at line 325 of file navsat_transform.h.
|
private |
Whether we use a Local Cartesian (tangent plane ENU) or the UTM coordinates as our cartesian.
Definition at line 211 of file navsat_transform.h.
|
private |
Whether we get our datum from the first GPS message or from the set_datum service/parameter.
Definition at line 203 of file navsat_transform.h.
|
private |
Whether we get the transform's yaw from the odometry or IMU source.
Definition at line 207 of file navsat_transform.h.
|
private |
UTM's meridian convergence.
Angle between projected meridian (True North) and UTM's grid Y-axis. For UTM projection (Ellipsoidal Transverse Mercator) it is zero on the equator and non-zero everywhere else. It increases as the poles are approached or as we're getting farther from central meridian.
Definition at line 241 of file navsat_transform.h.
|
private |
the UTM zone (zero means UPS)
Definition at line 267 of file navsat_transform.h.
|
private |
Frame ID of the GPS odometry output.
This will just match whatever your odometry message has
Definition at line 277 of file navsat_transform.h.
|
private |
IMU's yaw offset.
Your IMU should read 0 when facing magnetic north. If it doesn't, this (parameterized) value gives the offset (NOTE: if you have a magenetic declination, use the parameter setting for that).
Definition at line 248 of file navsat_transform.h.
|
private |
Whether or not to report 0 altitude.
If this parameter is true, we always report 0 for the altitude of the converted GPS odometry message.
Definition at line 229 of file navsat_transform.h.