本文整理汇总了C++中UAS::get_gps_fix方法的典型用法代码示例。如果您正苦于以下问题：C++ UAS::get_gps_fix方法的具体用法？C++ UAS::get_gps_fix怎么用？C++ UAS::get_gps_fix使用的例子？那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类UAS的用法示例。
在下文中一共展示了UAS::get_gps_fix方法的1个代码示例，这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞，您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: handle_global_position_int
void handle_global_position_int(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_global_position_int_t gpos;
mavlink_msg_global_position_int_decode(msg, &gpos);
auto pose_cov = boost::make_shared<geometry_msgs::PoseWithCovarianceStamped>();
auto fix = boost::make_shared<sensor_msgs::NavSatFix>();
auto vel = boost::make_shared<geometry_msgs::TwistStamped>();
auto relative_alt = boost::make_shared<std_msgs::Float64>();
auto compass_heading = boost::make_shared<std_msgs::Float64>();
auto header = uas->synchronized_header(frame_id, gpos.time_boot_ms);
// Global position fix
fix->header = header;
fill_lla(gpos, fix);
// fill GPS status fields using GPS_RAW data
auto raw_fix = uas->get_gps_fix();
if (raw_fix) {
fix->status.service = raw_fix->status.service;
fix->status.status = raw_fix->status.status;
fix->position_covariance = raw_fix->position_covariance;
fix->position_covariance_type = raw_fix->position_covariance_type;
}
else {
// no GPS_RAW_INT -> fix status unknown
fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
// we don't know covariance
fill_unknown_cov(fix);
}
/* Global position velocity
*
* No transform needed:
* X: latitude m/s
* Y: longitude m/s
* Z: altitude m/s
*/
vel->header = header;
tf::vectorEigenToMsg(
Eigen::Vector3d(gpos.vx, gpos.vy, gpos.vz) / 1E2,
vel->twist.linear);
relative_alt->data = gpos.relative_alt / 1E3; // in meters
compass_heading->data = (gpos.hdg != UINT16_MAX) ? gpos.hdg / 1E2 : NAN; // in degrees
// local position (UTM) calculation
double northing, easting;
std::string zone;
/** @note Adapted from gps_umd ROS package @http://wiki.ros.org/gps_umd
* Author: Ken Tossell <ken AT tossell DOT net>
*/
UTM::LLtoUTM(fix->latitude, fix->longitude, northing, easting, zone);
pose_cov->header = header;
pose_cov->pose.pose.position.x = easting;
pose_cov->pose.pose.position.y = northing;
pose_cov->pose.pose.position.z = relative_alt->data;
// XXX FIX ME #319, We really need attitude on UTM?
tf::quaternionTFToMsg(uas->get_attitude_orientation(), pose_cov->pose.pose.orientation);
// Use ENU covariance to build XYZRPY covariance
UAS::EigenMapConstCovariance3d gps_cov(fix->position_covariance.data());
UAS::EigenMapCovariance6d cov_out(pose_cov->pose.covariance.data());
cov_out <<
gps_cov(0, 0) , gps_cov(0, 1) , gps_cov(0, 2) , 0.0 , 0.0 , 0.0 ,
gps_cov(1, 0) , gps_cov(1, 1) , gps_cov(1, 2) , 0.0 , 0.0 , 0.0 ,
gps_cov(2, 0) , gps_cov(2, 1) , gps_cov(2, 2) , 0.0 , 0.0 , 0.0 ,
0.0 , 0.0 , 0.0 , rot_cov , 0.0 , 0.0 ,
0.0 , 0.0 , 0.0 , 0.0 , rot_cov , 0.0 ,
0.0 , 0.0 , 0.0 , 0.0 , 0.0 , rot_cov ;
// publish
gp_fix_pub.publish(fix);
gp_pos_pub.publish(pose_cov);
gp_vel_pub.publish(vel);
gp_rel_alt_pub.publish(relative_alt);
gp_hdg_pub.publish(compass_heading);
// TF
if (tf_send) {
geometry_msgs::TransformStamped transform;
transform.header.stamp = pose_cov->header.stamp;
transform.header.frame_id = tf_frame_id;
transform.child_frame_id = tf_child_frame_id;
// XXX do we really need rotation in UTM coordinates?
// setRotation()
transform.transform.rotation = pose_cov->pose.pose.orientation;
// setOrigin(), why to do transform_frame?
transform.transform.translation.x = pose_cov->pose.pose.position.x;
transform.transform.translation.y = pose_cov->pose.pose.position.y;
transform.transform.translation.z = pose_cov->pose.pose.position.z;
//.........这里部分代码省略.........