[WIP] Set framework to define offset between global origin and current local position

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
 )
 

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

mavros/package.xml

@@ -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>
 

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 <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.
  *
@@ -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);
@@ -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),
+			       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,25 +101,47 @@ 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;
 
 	template<typename MsgT>
-	inline void fill_lla(MsgT &msg, sensor_msgs::NavSatFix::Ptr fix) {
+	inline void fill_lla_wgs84(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
 	}
 
+	template<typename MsgT>
+	inline void fill_lla_ecef(MsgT &msg, geographic_msgs::GeoPointStamped::Ptr point) {
+		// @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>
+		point->position.latitude = msg.latitude / 1E7;		// deg
+		point->position.longitude = msg.longitude / 1E7;	// deg
+		point->position.altitude = msg.altitude / 1E3;		// m
+	}
+
+	template<typename MsgT>
+	inline void fill_lla_amsl(const geographic_msgs::GeoPointStamped::ConstPtr point, MsgT &msg) {
+		// @todo: add convertion from ECEF to AMSL
+		msg.latitude = point->position.latitude * 1E7;		// deg
+		msg.longitude = point->position.longitude * 1E7;	// deg
+		msg.altitude = point->position.altitude * 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;
+					sensor_msgs::NavSatFix::COVARIANCE_TYPE_UNKNOWN;
 	}
 
 	/* -*- message handlers -*- */
@@ -127,7 +160,7 @@ class GlobalPositionPlugin : public plugin::PluginBase {
 			fix->status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX;
 		}
 
-		fill_lla(raw_gps, fix);
+		fill_lla_wgs84(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;
@@ -137,10 +170,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 +184,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 +201,18 @@ 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();
+		fill_lla_ecef(glob_orig, g_origin);	// @warning TODO: #693
+
+		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)
@@ -182,7 +227,7 @@ class GlobalPositionPlugin : public plugin::PluginBase {
 		// Global position fix
 		fix->header = header;
 
-		fill_lla(gpos, fix);
+		fill_lla_wgs84(gpos, fix);
 
 		// fill GPS status fields using GPS_RAW data
 		auto raw_fix = m_uas->get_gps_fix();
@@ -218,8 +263,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 +288,9 @@ 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;
 
 		// publish
 		gp_fix_pub.publish(fix);
@@ -277,6 +318,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)
 	{
@@ -307,6 +383,19 @@ 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
+		fill_lla_amsl(req, gpo);// @warning TODO: #693
+
+		UAS_FCU(m_uas)->send_message_ignore_drop(gpo);
+	}
 };
 }	// namespace std_plugins
 }	// namespace mavros

mavros/src/plugins/hil_actuator_controls.cpp

@@ -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)
-