diff --git a/mavros/CMakeLists.txt b/mavros/CMakeLists.txt
index 3b3a0bda2..7ff242503 100644
--- a/mavros/CMakeLists.txt
+++ b/mavros/CMakeLists.txt
@@ -12,6 +12,7 @@ find_package(catkin REQUIRED COMPONENTS
geometry_msgs
sensor_msgs
nav_msgs
+ geographic_msgs
std_msgs
std_srvs
tf2_ros
@@ -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
)
diff --git a/mavros/launch/px4_config.yaml b/mavros/launch/px4_config.yaml
index e8525f333..31872de46 100644
--- a/mavros/launch/px4_config.yaml
+++ b/mavros/launch/px4_config.yaml
@@ -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
diff --git a/mavros/package.xml b/mavros/package.xml
index d152bf87c..93cb01691 100644
--- a/mavros/package.xml
+++ b/mavros/package.xml
@@ -48,6 +48,7 @@
mavros_msgs
nav_msgs
sensor_msgs
+ geographic_msgs
std_msgs
std_srvs
diff --git a/mavros/src/plugins/global_position.cpp b/mavros/src/plugins/global_position.cpp
index 16ef92d89..8b7e06b53 100644
--- a/mavros/src/plugins/global_position.cpp
+++ b/mavros/src/plugins/global_position.cpp
@@ -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
@@ -25,13 +25,13 @@
#include
#include
#include
+#include
#include
#include
+#include
namespace mavros {
namespace std_plugins {
-
-
/**
* @brief Global position plugin.
*
@@ -57,6 +57,7 @@ class GlobalPositionPlugin : public plugin::PluginBase {
// tf subsection
gp_nh.param("tf/send", tf_send, true);
gp_nh.param("tf/frame_id", tf_frame_id, "map");
+ gp_nh.param("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("tf/child_frame_id", tf_child_frame_id, "base_link");
UAS_DIAG(m_uas).add("GPS", this, &GlobalPositionPlugin::gps_diag_run);
@@ -70,6 +71,14 @@ class GlobalPositionPlugin : public plugin::PluginBase {
gp_odom_pub = gp_nh.advertise("local", 10);
gp_rel_alt_pub = gp_nh.advertise("rel_alt", 10);
gp_hdg_pub = gp_nh.advertise("compass_hdg", 10);
+
+ // global origin
+ gp_global_origin_pub = gp_nh.advertise("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("gp_lp_offset", 10);
+
}
Subscriptions get_subscriptions()
@@ -77,7 +86,9 @@ class GlobalPositionPlugin : public plugin::PluginBase {
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)
};
}
@@ -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;
@@ -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 -*- */
@@ -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);
@@ -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
@@ -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();
+ // 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
+ 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)
@@ -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());
@@ -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);
@@ -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();
+ 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)
{
@@ -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
diff --git a/mavros/src/plugins/hil_actuator_controls.cpp b/mavros/src/plugins/hil_actuator_controls.cpp
index ef5908d8d..cd2f62c83 100644
--- a/mavros/src/plugins/hil_actuator_controls.cpp
+++ b/mavros/src/plugins/hil_actuator_controls.cpp
@@ -70,4 +70,3 @@ class HilActuatorControlsPlugin : public plugin::PluginBase {
#include
PLUGINLIB_EXPORT_CLASS(mavros::std_plugins::HilActuatorControlsPlugin, mavros::plugin::PluginBase)
-