Skip to content

Commit

Permalink
plugin: global_position #325: merge gps_raw_int handler
Browse files Browse the repository at this point in the history
  • Loading branch information
@vooon
vooon committed Jul 2, 2015
1 parent f7bbb49 commit 4c22439
Showing 1 changed file with 193 additions and 67 deletions.
260 changes: 193 additions & 67 deletions mavros/src/plugins/global_position.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,27 +9,30 @@
*/
/*
* Copyright 2014 Nuno Marques.
* Copyright 2015 Vladimir Ermakov.
*
* This file is part of the mavros package and subject to the license terms
* in the top-level LICENSE file of the mavros repository.
* https://github.com/mavlink/mavros/tree/master/LICENSE.md
*/

#include <angles/angles.h>
#include <mavros/mavros_plugin.h>
#include <mavros/gps_conversions.h>
#include <pluginlib/class_list_macros.h>
#include <eigen_conversions/eigen_msg.h>
#include <tf2_ros/transform_broadcaster.h>

#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <std_msgs/Float64.h>
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/NavSatStatus.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/Quaternion.h>
#include <std_msgs/Float64.h>
#include <std_msgs/Header.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/TransformStamped.h>

namespace mavplugin {


/**
* @brief Global position plugin.
*
Expand All @@ -42,114 +45,202 @@ class GlobalPositionPlugin : public MavRosPlugin {
GlobalPositionPlugin() :
gp_nh("~global_position"),
uas(nullptr),
send_tf(false),
tf_send(false),
rot_cov(99999.0)
{ };

void initialize(UAS &uas_)
{
uas = &uas_;

gp_nh.param("send_tf", send_tf, true);
gp_nh.param<std::string>("frame_id", frame_id, "local_origin");
gp_nh.param<std::string>("child_frame_id", child_frame_id, "fcu");
gp_nh.param<double>("rot_covariance", rot_cov, 99999.0);

fix_pub = gp_nh.advertise<sensor_msgs::NavSatFix>("global", 10);
pos_pub = gp_nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("local", 10);
vel_pub = gp_nh.advertise<geometry_msgs::Vector3Stamped>("gp_vel", 10);
rel_alt_pub = gp_nh.advertise<std_msgs::Float64>("rel_alt", 10);
hdg_pub = gp_nh.advertise<std_msgs::Float64>("compass_hdg", 10);
// general params
gp_nh.param<std::string>("frame_id", frame_id, "fcu");
gp_nh.param("rot_covariance", rot_cov, 99999.0);
// tf subsection
gp_nh.param("tf/send", tf_send, true);
gp_nh.param<std::string>("tf/frame_id", tf_frame_id, "local_origin");
gp_nh.param<std::string>("tf/child_frame_id", tf_child_frame_id, "fcu");

UAS_DIAG(uas).add("GPS", this, &GlobalPositionPlugin::gps_diag_run);

// gps data
raw_fix_pub = gp_nh.advertise<sensor_msgs::NavSatFix>("raw/fix", 10);
raw_vel_pub = gp_nh.advertise<geometry_msgs::TwistStamped>("raw/gps_vel", 10);

// fused global position
gp_fix_pub = gp_nh.advertise<sensor_msgs::NavSatFix>("global", 10);
//gp_pos_pub = gp_nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("local", 10);
gp_vel_pub = gp_nh.advertise<geometry_msgs::TwistStamped>("gp_vel", 10);
gp_rel_alt_pub = gp_nh.advertise<std_msgs::Float64>("rel_alt", 10);
gp_hdg_pub = gp_nh.advertise<std_msgs::Float64>("compass_hdg", 10);
}

const message_map get_rx_handlers() {
return {
MESSAGE_HANDLER(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &GlobalPositionPlugin::handle_global_position_int)
MESSAGE_HANDLER(MAVLINK_MSG_ID_GPS_RAW_INT, &GlobalPositionPlugin::handle_gps_raw_int),
// MAVLINK_MSG_ID_GPS_STATUS: there no corresponding ROS message, and it is not supported by APM
MESSAGE_HANDLER(MAVLINK_MSG_ID_GLOBAL_POSITION_INT, &GlobalPositionPlugin::handle_global_position_int)
};
}

private:
ros::NodeHandle gp_nh;
UAS *uas;

ros::Publisher fix_pub;
ros::Publisher pos_pub;
ros::Publisher vel_pub;
ros::Publisher hdg_pub;
ros::Publisher rel_alt_pub;
ros::Publisher raw_fix_pub;
ros::Publisher raw_vel_pub;
ros::Publisher gp_fix_pub;
ros::Publisher gp_pos_pub;
ros::Publisher gp_vel_pub;
ros::Publisher gp_hdg_pub;
ros::Publisher gp_rel_alt_pub;

tf::TransformBroadcaster tf_broadcaster;
//tf::TransformBroadcaster tf_broadcaster;

std::string frame_id; //!< origin for TF
std::string child_frame_id; //!< frame for TF and Pose
bool send_tf;
std::string frame_id; //!< frame for topic headers
std::string tf_frame_id; //!< origin for TF
std::string tf_child_frame_id; //!< frame for TF and Pose
bool tf_send;
double rot_cov;

template<typename MsgT>
inline void fill_lla(MsgT &msg, sensor_msgs::NavSatFix::Ptr fix) {
fix->latitude = msg.lat / 1E7; // deg
fix->longitude = msg.lon / 1E7; // deg
fix->altitude = msg.alt / 1E3; // m
}

inline void fill_unknown_cov(sensor_msgs::NavSatFix::Ptr fix) {
fix->position_covariance.fill(0.0);
fix->position_covariance[0] = -1.0;
fix->position_covariance_type =
sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
}

/* -*- message handlers -*- */

void handle_gps_raw_int(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_gps_raw_int_t raw_gps;
mavlink_msg_gps_raw_int_decode(msg, &raw_gps);

auto fix = boost::make_shared<sensor_msgs::NavSatFix>();

fix->header.frame_id = frame_id;
fix->header.stamp = uas->synchronise_stamp(raw_gps.time_usec);

fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
if (raw_gps.fix_type > 2)
fix->status.status = sensor_msgs::NavSatStatus::STATUS_FIX;
else {
ROS_WARN_THROTTLE_NAMED(30, "global_position", "GP: No GPS fix");
fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
}

fill_lla(raw_gps, fix);

float eph = (raw_gps.eph != UINT16_MAX) ? raw_gps.eph / 1E2F : NAN;
float epv = (raw_gps.epv != UINT16_MAX) ? raw_gps.epv / 1E2F : NAN;

if (!isnan(eph)) {
const double hdop = eph;

// From nmea_navsat_driver
fix->position_covariance[0 + 0] = \
fix->position_covariance[3 + 1] = std::pow(hdop, 2);
fix->position_covariance[6 + 2] = std::pow(2 * hdop, 2);
fix->position_covariance_type =
sensor_msgs::NavSatFix::COVARIANCE_TYPE_APPROXIMATED;
}
else {
fill_unknown_cov(fix);
}

// store & publish
uas->update_gps_fix_epts(fix, eph, epv, raw_gps.fix_type, raw_gps.satellites_visible);
raw_fix_pub.publish(fix);

if (raw_gps.vel != UINT16_MAX &&
raw_gps.cog != UINT16_MAX) {
double speed = raw_gps.vel / 1E2; // m/s
double course = angles::from_degrees(raw_gps.cog / 1E2); // rad

auto vel = boost::make_shared<geometry_msgs::TwistStamped>();

vel->header.frame_id = frame_id;
vel->header.stamp = fix->header.stamp;

// From nmea_navsat_driver
vel->twist.linear.x = speed * std::sin(course);
vel->twist.linear.y = speed * std::cos(course);

raw_vel_pub.publish(vel);
}
}

/** @todo Handler for GLOBAL_POSITION_INT_COV */

void handle_global_position_int(const mavlink_message_t *msg, uint8_t sysid, uint8_t compid) {
mavlink_global_position_int_t gp_pos;
mavlink_msg_global_position_int_decode(msg, &gp_pos);

ROS_DEBUG_THROTTLE_NAMED(10, "global_position", "Global position: boot_ms:%06d "
"lat/lon/alt:(%d %d %d) relative_alt: (%d) gp_vel:(%d %d %d) compass_heading: (%d)",
gp_pos.time_boot_ms,
gp_pos.lat, gp_pos.lon, gp_pos.alt, gp_pos.relative_alt,
gp_pos.vx, gp_pos.vy, gp_pos.vz, gp_pos.hdg);
mavlink_global_position_int_t gpos;
mavlink_msg_global_position_int_decode(msg, &gpos);

auto pose_cov = boost::make_shared<geometry_msgs::PoseWithCovarianceStamped>();
auto gp_fix = boost::make_shared<sensor_msgs::NavSatFix>();
auto gp_vel = boost::make_shared<geometry_msgs::Vector3Stamped>();
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>();

std_msgs::Header header;
header.frame_id = frame_id;
header.stamp = uas->synchronise_stamp(gp_pos.time_boot_ms);
header.stamp = uas->synchronise_stamp(gpos.time_boot_ms);

// Global position fix
gp_fix->header = header;
gp_fix->latitude = gp_pos.lat / 1E7;
gp_fix->longitude = gp_pos.lon / 1E7;
gp_fix->altitude = gp_pos.alt / 1E3; // in meters
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) {
gp_fix->status.service = raw_fix->status.service;
gp_fix->status.status = raw_fix->status.status;
gp_fix->position_covariance = raw_fix->position_covariance;
gp_fix->position_covariance_type = raw_fix->position_covariance_type;
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
gp_fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
gp_fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
fix->status.service = sensor_msgs::NavSatStatus::SERVICE_GPS;
fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;

// we don't know covariance
std::fill(gp_fix->position_covariance.begin(),
gp_fix->position_covariance.end(), 0.0);
gp_fix->position_covariance[0] = -1.0;
gp_fix->position_covariance_type =
sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
fill_unknown_cov(fix);
}

// Global position velocity
gp_vel->header = header;
gp_vel->vector.x = gp_pos.vx / 1E2; // in m/s
gp_vel->vector.y = gp_pos.vy / 1E2;
gp_vel->vector.z = gp_pos.vz / 1E2;
/* 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 = gp_pos.relative_alt / 1E3; // in meters
compass_heading->data = (gp_pos.hdg != UINT16_MAX) ? gp_pos.hdg / 1E2 : NAN; // in degrees
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;

#if 0
// temporary comment out

/** @note Adapted from gps_umd ROS package @http://wiki.ros.org/gps_umd
* Author: Ken Tossell <ken AT tossell DOT net>
*/
UTM::LLtoUTM(gp_fix->latitude, gp_fix->longitude, northing, easting, zone);
UTM::LLtoUTM(gfix->latitude, gfix->longitude, northing, easting, zone);

pose_cov->header = header;
pose_cov->pose.pose.position.x = easting;
Expand Down Expand Up @@ -178,13 +269,17 @@ class GlobalPositionPlugin : public MavRosPlugin {
};

pose_cov->pose.covariance = covariance;
#endif

fix_pub.publish(gp_fix);
pos_pub.publish(pose_cov);
vel_pub.publish(gp_vel);
rel_alt_pub.publish(relative_alt);
hdg_pub.publish(compass_heading);
// 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);

#if 0
// need tf2

This comment has been minimized.

Sign in to view

Copy link
@TSC21

TSC21 Jul 2, 2015

Member

So for when TF2 then? :P
if (send_tf) {
tf::Transform transform;

Expand All @@ -202,6 +297,37 @@ class GlobalPositionPlugin : public MavRosPlugin {
pose_cov->header.stamp,
frame_id, child_frame_id));
}
#endif
}

/* -*- diagnostics -*- */
void gps_diag_run(diagnostic_updater::DiagnosticStatusWrapper &stat) {
int fix_type, satellites_visible;
float eph, epv;

uas->get_gps_epts(eph, epv, fix_type, satellites_visible);

if (satellites_visible <= 0)
stat.summary(2, "No satellites");
else if (fix_type < 2)
stat.summary(1, "No fix");
else if (fix_type == 2)
stat.summary(0, "2D fix");
else if (fix_type >= 3)
stat.summary(0, "3D fix");

stat.addf("Satellites visible", "%zd", satellites_visible);
stat.addf("Fix type", "%d", fix_type);

if (!isnan(eph))
stat.addf("EPH (m)", "%.2f", eph);
else
stat.add("EPH (m)", "Unknown");

if (!isnan(epv))
stat.addf("EPV (m)", "%.2f", epv);
else
stat.add("EPV (m)", "Unknown");
}
};
}; // namespace mavplugin
Expand Down

0 comments on commit 4c22439

Please sign in to comment.