#include <navsat_transform.h>
Public Member Functions | |
| NavSatTransform () | |
| Constructor. | |
| void | run () |
| Main run loop. | |
| ~NavSatTransform () | |
| Destructor. | |
Private Member Functions | |
| void | computeTransform () |
| Computes the transform from the UTM frame to the odom frame. | |
| bool | datumCallback (robot_localization::SetDatum::Request &request, robot_localization::SetDatum::Response &) |
| Callback for the datum service. | |
| void | getRobotOriginUtmPose (const tf2::Transform &gps_utm_pose, tf2::Transform &robot_utm_pose, const ros::Time &transform_time) |
| Given the pose of the navsat sensor in the UTM frame, removes the offset from the vehicle's centroid and returns the UTM-frame pose of said centroid. | |
| 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. | |
| void | gpsFixCallback (const sensor_msgs::NavSatFixConstPtr &msg) |
| Callback for the GPS fix data. | |
| void | imuCallback (const sensor_msgs::ImuConstPtr &msg) |
| Callback for the IMU data. | |
| void | odomCallback (const nav_msgs::OdometryConstPtr &msg) |
| Callback for the odom data. | |
| bool | prepareFilteredGps (sensor_msgs::NavSatFix &filtered_gps) |
| Converts the odometry data back to GPS and broadcasts it. | |
| bool | prepareGpsOdometry (nav_msgs::Odometry &gps_odom) |
| Prepares the GPS odometry message before sending. | |
| void | setTransformGps (const sensor_msgs::NavSatFixConstPtr &msg) |
| Used for setting the GPS data that will be used to compute the transform. | |
| void | setTransformOdometry (const nav_msgs::OdometryConstPtr &msg) |
| Used for setting the odometry data that will be used to compute the transform. | |
Private Attributes | |
| std::string | base_link_frame_id_ |
| Frame ID of the robot's body frame. | |
| bool | broadcast_utm_transform_ |
| Whether or not we broadcast the UTM transform. | |
| std::string | gps_frame_id_ |
| The frame_id of the GPS message (specifies mounting location) | |
| ros::Time | gps_update_time_ |
| Timestamp of the latest good GPS message. | |
| bool | gps_updated_ |
| Whether or not we have new GPS data. | |
| bool | has_transform_gps_ |
| Whether or not the GPS fix is usable. | |
| bool | has_transform_imu_ |
| Signifies that we have received a usable IMU message. | |
| bool | has_transform_odom_ |
| Signifies that we have received a usable odometry message. | |
| Eigen::MatrixXd | latest_odom_covariance_ |
| Covariance for most recent odometry data. | |
| Eigen::MatrixXd | latest_utm_covariance_ |
| Covariance for most recent GPS/UTM data. | |
| tf2::Transform | latest_utm_pose_ |
| Latest GPS data, stored as UTM coords. | |
| tf2::Transform | latest_world_pose_ |
| Latest odometry pose data. | |
| double | magnetic_declination_ |
| Parameter that specifies the magnetic declination for the robot's environment. | |
| ros::Time | odom_update_time_ |
| Timestamp of the latest good odometry message. | |
| bool | odom_updated_ |
| Whether or not we have new odometry data. | |
| bool | publish_gps_ |
| Whether or not we publish filtered GPS messages. | |
| tf2_ros::Buffer | tf_buffer_ |
| Transform buffer for managing coordinate transforms. | |
| tf2_ros::TransformListener | tf_listener_ |
| Transform listener for receiving transforms. | |
| bool | transform_good_ |
| Whether or not we've computed a good heading. | |
| tf2::Quaternion | transform_orientation_ |
| Latest IMU orientation. | |
| ros::Duration | transform_timeout_ |
| Parameter that specifies the how long we wait for a transform to become available. | |
| tf2::Transform | transform_utm_pose_ |
| Holds the UTM pose that is used to compute the transform. | |
| tf2::Transform | transform_world_pose_ |
| Latest IMU orientation. | |
| bool | use_manual_datum_ |
| Whether we get our datum from the first GPS message or from the set_datum service/parameter. | |
| bool | use_odometry_yaw_ |
| Whether we get the transform's yaw from the odometry or IMU source. | |
| tf2_ros::StaticTransformBroadcaster | utm_broadcaster_ |
| Used for publishing the static world_frame->utm transform. | |
| double | utm_odom_tf_yaw_ |
| Stores the yaw we need to compute the transform. | |
| tf2::Transform | utm_world_trans_inverse_ |
| Holds the odom->UTM transform for filtered GPS broadcast. | |
| tf2::Transform | utm_world_transform_ |
| Holds the UTM->odom transform. | |
| std::string | utm_zone_ |
| UTM zone as determined after transforming GPS message. | |
| std::string | world_frame_id_ |
| Frame ID of the GPS odometry output. | |
| double | yaw_offset_ |
| IMU's yaw offset. | |
| bool | zero_altitude_ |
| Whether or not to report 0 altitude. | |
Definition at line 56 of file navsat_transform.h.
Constructor.
Definition at line 47 of file navsat_transform.cpp.
Destructor.
Definition at line 73 of file navsat_transform.cpp.
| void RobotLocalization::NavSatTransform::computeTransform | ( | ) | [private] |
Computes the transform from the UTM frame to the odom frame.
Definition at line 220 of file navsat_transform.cpp.
| bool RobotLocalization::NavSatTransform::datumCallback | ( | robot_localization::SetDatum::Request & | request, |
| robot_localization::SetDatum::Response & | |||
| ) | [private] |
Callback for the datum service.
Definition at line 307 of file navsat_transform.cpp.
| void RobotLocalization::NavSatTransform::getRobotOriginUtmPose | ( | const tf2::Transform & | gps_utm_pose, |
| tf2::Transform & | robot_utm_pose, | ||
| const ros::Time & | transform_time | ||
| ) | [private] |
Given the pose of the navsat sensor in the UTM frame, removes the offset from the vehicle's centroid and returns the UTM-frame pose of said centroid.
Definition at line 351 of file navsat_transform.cpp.
| void RobotLocalization::NavSatTransform::getRobotOriginWorldPose | ( | const tf2::Transform & | gps_odom_pose, |
| tf2::Transform & | robot_odom_pose, | ||
| const ros::Time & | transform_time | ||
| ) | [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 401 of file navsat_transform.cpp.
| void RobotLocalization::NavSatTransform::gpsFixCallback | ( | const sensor_msgs::NavSatFixConstPtr & | msg | ) | [private] |
Callback for the GPS fix data.
| [in] | msg | The NavSatFix message to process |
Definition at line 444 of file navsat_transform.cpp.
| void RobotLocalization::NavSatTransform::imuCallback | ( | const sensor_msgs::ImuConstPtr & | msg | ) | [private] |
Callback for the IMU data.
| [in] | msg | The IMU message to process |
Definition at line 490 of file navsat_transform.cpp.
| void RobotLocalization::NavSatTransform::odomCallback | ( | const nav_msgs::OdometryConstPtr & | msg | ) | [private] |
Callback for the odom data.
| [in] | msg | The odometry message to process |
Definition at line 545 of file navsat_transform.cpp.
| bool RobotLocalization::NavSatTransform::prepareFilteredGps | ( | sensor_msgs::NavSatFix & | filtered_gps | ) | [private] |
Converts the odometry data back to GPS and broadcasts it.
| [out] | filtered_gps | The NavSatFix message to prepare |
Definition at line 570 of file navsat_transform.cpp.
| bool RobotLocalization::NavSatTransform::prepareGpsOdometry | ( | nav_msgs::Odometry & | gps_odom | ) | [private] |
Prepares the GPS odometry message before sending.
| [out] | gps_odom | The odometry message to prepare |
Definition at line 629 of file navsat_transform.cpp.
Main run loop.
Definition at line 77 of file navsat_transform.cpp.
| void RobotLocalization::NavSatTransform::setTransformGps | ( | const sensor_msgs::NavSatFixConstPtr & | msg | ) | [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 687 of file navsat_transform.cpp.
| void RobotLocalization::NavSatTransform::setTransformOdometry | ( | const nav_msgs::OdometryConstPtr & | msg | ) | [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 702 of file navsat_transform.cpp.
std::string RobotLocalization::NavSatTransform::base_link_frame_id_ [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 133 of file navsat_transform.h.
bool RobotLocalization::NavSatTransform::broadcast_utm_transform_ [private] |
Whether or not we broadcast the UTM transform.
Definition at line 137 of file navsat_transform.h.
std::string RobotLocalization::NavSatTransform::gps_frame_id_ [private] |
The frame_id of the GPS message (specifies mounting location)
Definition at line 141 of file navsat_transform.h.
Timestamp of the latest good GPS message.
We assign this value to the timestamp of the odometry message that we output
Definition at line 147 of file navsat_transform.h.
bool RobotLocalization::NavSatTransform::gps_updated_ [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 153 of file navsat_transform.h.
bool RobotLocalization::NavSatTransform::has_transform_gps_ [private] |
Whether or not the GPS fix is usable.
Definition at line 157 of file navsat_transform.h.
bool RobotLocalization::NavSatTransform::has_transform_imu_ [private] |
Signifies that we have received a usable IMU message.
Definition at line 161 of file navsat_transform.h.
bool RobotLocalization::NavSatTransform::has_transform_odom_ [private] |
Signifies that we have received a usable odometry message.
Definition at line 165 of file navsat_transform.h.
Eigen::MatrixXd RobotLocalization::NavSatTransform::latest_odom_covariance_ [private] |
Covariance for most recent odometry data.
Definition at line 169 of file navsat_transform.h.
Eigen::MatrixXd RobotLocalization::NavSatTransform::latest_utm_covariance_ [private] |
Covariance for most recent GPS/UTM data.
Definition at line 173 of file navsat_transform.h.
Latest GPS data, stored as UTM coords.
Definition at line 177 of file navsat_transform.h.
Latest odometry pose data.
Definition at line 181 of file navsat_transform.h.
double RobotLocalization::NavSatTransform::magnetic_declination_ [private] |
Parameter that specifies the magnetic declination for the robot's environment.
Definition at line 185 of file navsat_transform.h.
Timestamp of the latest good odometry message.
We assign this value to the timestamp of the odometry message that we output
Definition at line 191 of file navsat_transform.h.
bool RobotLocalization::NavSatTransform::odom_updated_ [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 197 of file navsat_transform.h.
bool RobotLocalization::NavSatTransform::publish_gps_ [private] |
Whether or not we publish filtered GPS messages.
Definition at line 201 of file navsat_transform.h.
Transform buffer for managing coordinate transforms.
Definition at line 205 of file navsat_transform.h.
Transform listener for receiving transforms.
Definition at line 209 of file navsat_transform.h.
bool RobotLocalization::NavSatTransform::transform_good_ [private] |
Whether or not we've computed a good heading.
Definition at line 213 of file navsat_transform.h.
Latest IMU orientation.
Definition at line 217 of file navsat_transform.h.
Parameter that specifies the how long we wait for a transform to become available.
Definition at line 271 of file navsat_transform.h.
Holds the UTM pose that is used to compute the transform.
Definition at line 221 of file navsat_transform.h.
Latest IMU orientation.
Definition at line 225 of file navsat_transform.h.
bool RobotLocalization::NavSatTransform::use_manual_datum_ [private] |
Whether we get our datum from the first GPS message or from the set_datum service/parameter.
Definition at line 230 of file navsat_transform.h.
bool RobotLocalization::NavSatTransform::use_odometry_yaw_ [private] |
Whether we get the transform's yaw from the odometry or IMU source.
Definition at line 234 of file navsat_transform.h.
Used for publishing the static world_frame->utm transform.
Definition at line 238 of file navsat_transform.h.
double RobotLocalization::NavSatTransform::utm_odom_tf_yaw_ [private] |
Stores the yaw we need to compute the transform.
Definition at line 242 of file navsat_transform.h.
Holds the odom->UTM transform for filtered GPS broadcast.
Definition at line 250 of file navsat_transform.h.
Holds the UTM->odom transform.
Definition at line 246 of file navsat_transform.h.
std::string RobotLocalization::NavSatTransform::utm_zone_ [private] |
UTM zone as determined after transforming GPS message.
Definition at line 254 of file navsat_transform.h.
std::string RobotLocalization::NavSatTransform::world_frame_id_ [private] |
Frame ID of the GPS odometry output.
This will just match whatever your odometry message has
Definition at line 260 of file navsat_transform.h.
double RobotLocalization::NavSatTransform::yaw_offset_ [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 267 of file navsat_transform.h.
bool RobotLocalization::NavSatTransform::zero_altitude_ [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 277 of file navsat_transform.h.