Skip to content

Commit

Permalink
[WIP] Set framework to define offset between global origin and curren…
Browse files Browse the repository at this point in the history 
…t local position (mavlink#691)

* add handlers for GPS_GLOBAL_ORIGIN and SET_GPS_GLOBAL_ORIGIN

* fix cast of encoding types

* refactor gps coord conversions

* uncrustify

* global_position: add LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET handler

* global_position: add trasform sender for offset

* global_origin: refactor covariance matrix

* global_position: update copyright

* global_position: add initial support to REP 105

* px4_config: global_position: update frame description

* global_position: correct identation

* global position: be consistent with frame and methods names (ecef!=wgs84, frame_id!=global_frame_id)

* global_position: updates to code structure

* global_position: fix identation
  • Loading branch information
@TSC21
TSC21 committed Aug 19, 2017
1 parent 41b3c4e commit 41fb6a9
Show file tree
Hide file tree
Showing 5 changed files with 105 additions and 19 deletions.
3 changes: 2 additions & 1 deletion mavros/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -12,6 +12,7 @@ find_package(catkin REQUIRED COMPONENTS
geometry_msgs
sensor_msgs
nav_msgs
geographic_msgs
std_msgs
std_srvs
tf2_ros
Expand Down Expand Up @@ -65,7 +66,7 @@ catkin_python_setup()
catkin_package(
INCLUDE_DIRS include
LIBRARIES mavros
CATKIN_DEPENDS diagnostic_msgs diagnostic_updater pluginlib roscpp sensor_msgs nav_msgs std_msgs tf2_ros geometry_msgs libmavconn message_runtime eigen_conversions mavros_msgs
CATKIN_DEPENDS diagnostic_msgs diagnostic_updater pluginlib roscpp sensor_msgs nav_msgs geographic_msgs std_msgs tf2_ros geometry_msgs libmavconn message_runtime eigen_conversions mavros_msgs
DEPENDS Boost EIGEN3
)

Expand Down
1 change: 1 addition & 0 deletions mavros/launch/px4_config.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,7 @@ global_position:
tf:
send: false # send TF?
frame_id: "local_origin" # TF frame_id
global_frame_id: "earth" # TF earth frame_id
child_frame_id: "fcu_utm" # TF child_frame_id

# imu_pub
Expand Down
1 change: 1 addition & 0 deletions mavros/package.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,7 @@
<depend>mavros_msgs</depend>
<depend>nav_msgs</depend>
<depend>sensor_msgs</depend>
<depend>geographic_msgs</depend>
<depend>std_msgs</depend>
<depend>std_srvs</depend>

Expand Down
118 changes: 101 additions & 17 deletions mavros/src/plugins/global_position.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,7 @@
* @{
*/
/*
* Copyright 2014 Nuno Marques.
* Copyright 2014,2017 Nuno Marques.
* Copyright 2015,2016 Vladimir Ermakov.
*
* This file is part of the mavros package and subject to the license terms
Expand All @@ -25,13 +25,13 @@
#include <nav_msgs/Odometry.h>
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/NavSatStatus.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <geographic_msgs/GeoPointStamped.h>

namespace mavros {
namespace std_plugins {


/**
* @brief Global position plugin.
*
Expand All @@ -57,6 +57,7 @@ class GlobalPositionPlugin : public plugin::PluginBase {
// tf subsection
gp_nh.param("tf/send", tf_send, true);
gp_nh.param<std::string>("tf/frame_id", tf_frame_id, "map");
gp_nh.param<std::string>("tf/global_frame_id", tf_global_frame_id, "earth"); // The global_origin should be represented as "earth" coordinate frame (ECEF) (REP 105)
gp_nh.param<std::string>("tf/child_frame_id", tf_child_frame_id, "base_link");

UAS_DIAG(m_uas).add("GPS", this, &GlobalPositionPlugin::gps_diag_run);
Expand All @@ -70,14 +71,24 @@ class GlobalPositionPlugin : public plugin::PluginBase {
gp_odom_pub = gp_nh.advertise<nav_msgs::Odometry>("local", 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);

// global origin
gp_global_origin_pub = gp_nh.advertise<geographic_msgs::GeoPointStamped>("gp_origin", 10);
gp_set_global_origin_sub = gp_nh.subscribe("set_gp_origin", 10, &GlobalPositionPlugin::set_gp_origin_cb, this);

// offset from local position to the global origin ("earth")
gp_global_offset_pub = gp_nh.advertise<geometry_msgs::PoseStamped>("gp_lp_offset", 10);

}

Subscriptions get_subscriptions()
{
return {
make_handler(&GlobalPositionPlugin::handle_gps_raw_int),
// GPS_STATUS: there no corresponding ROS message, and it is not supported by APM
make_handler(&GlobalPositionPlugin::handle_global_position_int)
make_handler(&GlobalPositionPlugin::handle_global_position_int),
make_handler(&GlobalPositionPlugin::handle_gps_global_origin),
make_handler(&GlobalPositionPlugin::handle_lpned_system_global_offset)
};
}

Expand All @@ -90,9 +101,14 @@ class GlobalPositionPlugin : public plugin::PluginBase {
ros::Publisher gp_fix_pub;
ros::Publisher gp_hdg_pub;
ros::Publisher gp_rel_alt_pub;
ros::Publisher gp_global_origin_pub;
ros::Publisher gp_global_offset_pub;

ros::Subscriber gp_set_global_origin_sub;

std::string frame_id; //!< frame for topic headers
std::string tf_frame_id; //!< origin for TF
std::string tf_global_frame_id; //!< global origin for TF
std::string tf_child_frame_id; //!< frame for TF and Pose
bool tf_send;
double rot_cov;
Expand All @@ -108,7 +124,7 @@ class GlobalPositionPlugin : public plugin::PluginBase {
fix->position_covariance.fill(0.0);
fix->position_covariance[0] = -1.0;
fix->position_covariance_type =
sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
}

/* -*- message handlers -*- */
Expand Down Expand Up @@ -137,10 +153,10 @@ class GlobalPositionPlugin : public plugin::PluginBase {

// From nmea_navsat_driver
fix->position_covariance[0 + 0] = \
fix->position_covariance[3 + 1] = std::pow(hdop, 2);
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;
sensor_msgs::NavSatFix::COVARIANCE_TYPE_APPROXIMATED;
}
else {
fill_unknown_cov(fix);
Expand All @@ -151,7 +167,7 @@ class GlobalPositionPlugin : public plugin::PluginBase {
raw_fix_pub.publish(fix);

if (raw_gps.vel != UINT16_MAX &&
raw_gps.cog != 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

Expand All @@ -168,6 +184,23 @@ class GlobalPositionPlugin : public plugin::PluginBase {
}
}

void handle_gps_global_origin(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GPS_GLOBAL_ORIGIN &glob_orig)
{
auto g_origin = boost::make_shared<geographic_msgs::GeoPointStamped>();
// auto header = m_uas->synchronized_header(frame_id, glob_orig.time_boot_ms); #TODO: requires Mavlink msg update

g_origin->header.frame_id = tf_global_frame_id;
g_origin->header.stamp = ros::Time::now();

// @todo: so to respect REP 105, we should convert from AMSL to ECEF using GeographicLib::GeoCoords (pending #693)
// see <http://www.ros.org/reps/rep-0105.html>
g_origin->position.latitude = glob_orig.latitude / 1E7; // deg
g_origin->position.longitude = glob_orig.longitude / 1E7; // deg
g_origin->position.altitude = glob_orig.altitude / 1E3; // m

gp_global_origin_pub.publish(g_origin);
}

/** @todo Handler for GLOBAL_POSITION_INT_COV */

void handle_global_position_int(const mavlink::mavlink_message_t *msg, mavlink::common::msg::GLOBAL_POSITION_INT &gpos)
Expand Down Expand Up @@ -218,8 +251,8 @@ class GlobalPositionPlugin : public plugin::PluginBase {

// Velocity
tf::vectorEigenToMsg(
Eigen::Vector3d(gpos.vx, gpos.vy, gpos.vz) / 1E2,
odom->twist.twist.linear);
Eigen::Vector3d(gpos.vx, gpos.vy, gpos.vz) / 1E2,
odom->twist.twist.linear);

// Velocity covariance unknown
ftf::EigenMapCovariance6d vel_cov_out(odom->twist.covariance.data());
Expand All @@ -243,13 +276,12 @@ class GlobalPositionPlugin : public plugin::PluginBase {
// Use ENU covariance to build XYZRPY covariance
ftf::EigenMapConstCovariance3d gps_cov(fix->position_covariance.data());
ftf::EigenMapCovariance6d pos_cov_out(odom->pose.covariance.data());
pos_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 ;
pos_cov_out.setZero();
pos_cov_out.block<3, 3>(0, 0) = gps_cov;
pos_cov_out.block<3, 3>(3, 3).diagonal() <<
rot_cov,
rot_cov,
rot_cov;

// publish
gp_fix_pub.publish(fix);
Expand Down Expand Up @@ -277,6 +309,41 @@ class GlobalPositionPlugin : public plugin::PluginBase {
}
}

void handle_lpned_system_global_offset(const mavlink::mavlink_message_t *msg, mavlink::common::msg::LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET &offset)
{
auto global_offset = boost::make_shared<geometry_msgs::PoseStamped>();
global_offset->header = m_uas->synchronized_header(tf_global_frame_id, offset.time_boot_ms);

auto enu_position = ftf::transform_frame_ned_enu(Eigen::Vector3d(offset.x, offset.y, offset.z));
auto enu_baselink_orientation = ftf::transform_orientation_aircraft_baselink(
ftf::transform_orientation_ned_enu(
ftf::quaternion_from_rpy(offset.roll, offset.pitch, offset.yaw)));

tf::pointEigenToMsg(enu_position, global_offset->pose.position);
tf::quaternionEigenToMsg(enu_baselink_orientation, global_offset->pose.orientation);

gp_global_offset_pub.publish(global_offset);

// TF
if (tf_send) {
geometry_msgs::TransformStamped transform;

transform.header.stamp = global_offset->header.stamp;
transform.header.frame_id = tf_global_frame_id;
transform.child_frame_id = tf_child_frame_id;

// setRotation()
transform.transform.rotation = global_offset->pose.orientation;

// setOrigin()
transform.transform.translation.x = global_offset->pose.position.x;
transform.transform.translation.y = global_offset->pose.position.y;
transform.transform.translation.z = global_offset->pose.position.z;

m_uas->tf2_broadcaster.sendTransform(transform);
}
}

/* -*- diagnostics -*- */
void gps_diag_run(diagnostic_updater::DiagnosticStatusWrapper &stat)
{
Expand Down Expand Up @@ -307,6 +374,23 @@ class GlobalPositionPlugin : public plugin::PluginBase {
else
stat.add("EPV (m)", "Unknown");
}

/* -*- callbacks -*- */

void set_gp_origin_cb(const geographic_msgs::GeoPointStamped::ConstPtr &req)
{
mavlink::common::msg::SET_GPS_GLOBAL_ORIGIN gpo;

gpo.target_system = m_uas->get_tgt_system();
// gpo.time_boot_ms = stamp.toNSec() / 1000; #TODO: requires Mavlink msg update

// @todo: add convertion from ECEF to AMSL #693
gpo.latitude = req->position.latitude * 1E7; // deg
gpo.longitude = req->position.longitude * 1E7; // deg
gpo.altitude = req->position.altitude * 1E3; // m

UAS_FCU(m_uas)->send_message_ignore_drop(gpo);
}
};
} // namespace std_plugins
} // namespace mavros
Expand Down
1 change: 0 additions & 1 deletion mavros/src/plugins/hil_actuator_controls.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -70,4 +70,3 @@ class HilActuatorControlsPlugin : public plugin::PluginBase {

#include <pluginlib/class_list_macros.h>
PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::HilActuatorControlsPlugin, mavros::plugin::PluginBase)

0 comments on commit 41fb6a9

Please sign in to comment.