Global position setpoint plugin

mavros/launch/px4_pluginlists.yaml

@@ -8,4 +8,5 @@ plugin_blacklist:
 - rangefinder
 
 plugin_whitelist: []
-#- 'sys_*'
+# 'sys_*'
+

mavros/src/plugins/setpoint_position.cpp

@@ -21,6 +21,10 @@
 #include <geometry_msgs/PoseStamped.h>
 
 #include <mavros_msgs/SetMavFrame.h>
+#include <mavros_msgs/GlobalPositionTarget.h>
+
+#include <GeographicLib/Geocentric.hpp>
+//#include <GeographicLib/Geoid.hpp>
 
 namespace mavros {
 namespace std_plugins {
@@ -36,6 +40,7 @@ class SetpointPositionPlugin : public plugin::PluginBase,
 public:
 	SetpointPositionPlugin() : PluginBase(),
 		sp_nh("~setpoint_position"),
+                spg_nh("~"),
 		tf_rate(50.0),
 		tf_listen(false)
 	{ }
@@ -57,6 +62,13 @@ class SetpointPositionPlugin : public plugin::PluginBase,
 		}
 		else {
 			setpoint_sub = sp_nh.subscribe("local", 10, &SetpointPositionPlugin::setpoint_cb, this);
+                        /* Subscriber for goal gps */
+                        setpointg_sub = sp_nh.subscribe("global", 10, &SetpointPositionPlugin::setpointg_cb, this);
+
+                        /* subscriber for current gps state, mavros/global_position/global. */
+                        gps_sub = spg_nh.subscribe("global_position/global", 10, &SetpointPositionPlugin::gps_cb, this);
+                        /* Subscribe for current local ENU pose. */
+                        local_sub = spg_nh.subscribe("local_position/pose", 10, &SetpointPositionPlugin::local_cb, this) ;
 		}
 		mav_frame_srv = sp_nh.advertiseService("mav_frame", &SetpointPositionPlugin::set_mav_frame_cb, this);
 
@@ -78,18 +90,28 @@ class SetpointPositionPlugin : public plugin::PluginBase,
 	friend class SetPositionTargetLocalNEDMixin;
 	friend class TF2ListenerMixin;
 	ros::NodeHandle sp_nh;
-
+        ros::NodeHandle spg_nh; //!< to get local position and gps coord which are not under sp_h()
 	ros::Subscriber setpoint_sub;
+        ros::Subscriber setpointg_sub; //!< GPS setpoint
+        ros::Subscriber gps_sub; //!< current GPS
+        ros::Subscriber local_sub; //!< current local ENU
 	ros::ServiceServer mav_frame_srv;
 
+        /* Stores current gps state. */
+        //sensor_msgs::NavSatFix current_gps_msg;
+        Eigen::Vector3d current_gps; /* geodetic coordinates LLA */
+        /* Current local position in ENU */
+        Eigen::Vector3d current_local_pos;
+        /* old time gps time stamp in [ms], to check if new gps msg is received */
+        uint32_t old_gps_stamp = 0;
+
 	std::string tf_frame_id;
 	std::string tf_child_frame_id;
 
 	bool tf_listen;
 	double tf_rate;
 
 	MAV_FRAME mav_frame;
-
 	/* -*- mid-level helpers -*- */
 
 	/**
@@ -124,7 +146,6 @@ class SetpointPositionPlugin : public plugin::PluginBase,
 							ftf::transform_orientation_baselink_aircraft(Eigen::Quaterniond(tr.rotation())));
 			}
 		} ();
-
 		set_position_target_local_ned(stamp.toNSec() / 1000000,
 					utils::enum_value(mav_frame),
 					ignore_all_except_xyz_y,
@@ -137,7 +158,7 @@ class SetpointPositionPlugin : public plugin::PluginBase,
 	/* -*- callbacks -*- */
 
 	/* common TF listener moved to mixin */
-	void transform_cb(const geometry_msgs::TransformStamped &transform) {
+        void transform_cb(const geometry_msgs::TransformStamped &transform) {
 		Eigen::Affine3d tr;
 		// TODO: later, when tf2 5.12 will be released need to revisit and replace this to
 		// tf2::convert()
@@ -153,6 +174,71 @@ class SetpointPositionPlugin : public plugin::PluginBase,
 		send_position_target(req->header.stamp, tr);
 	}
 
+        /* gets gps setpoint, converts it to local ENU, and sends it to FCU */
+        void setpointg_cb(const mavros_msgs::GlobalPositionTarget::ConstPtr &req){
+
+            /**
+              * The idea is to convert the change in LLA(goal_gps-current_gps) to change in ENU
+              * 1- convert current/goal gps points to current/goal ECEF points
+              * 2- claculate offset in ECEF frame
+              * 3- converts ECEF offset to ENU offset given current gps LLA
+              * 4- adds ENU offset to current local ENU to that will be sent to FCU
+              */
+
+            GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f());
+
+            Eigen::Vector3d goal_gps(req->latitude, req->longitude, req->altitude);
+
+            /* current gps -> curent ECEF */
+            Eigen::Vector3d current_ecef;
+            earth.Forward(current_gps.x(), current_gps.y(), current_gps.z(),
+            current_ecef.x(), current_ecef.y(), current_ecef.z());
+
+            /* goal gps -> goal ECEF */
+            Eigen::Vector3d goal_ecef;
+            earth.Forward(goal_gps.x(), goal_gps.y(), goal_gps.z(),
+            goal_ecef.x(), goal_ecef.y(), goal_ecef.z());
+
+            /* get ENU offset from ECEF offset */
+            Eigen::Vector3d ecef_offset = goal_ecef - current_ecef;
+            Eigen::Vector3d enu_offset = ftf::transform_frame_ecef_enu(ecef_offset, current_gps);
+
+            /* prepare yaw angle */
+            Eigen::Affine3d sp; /* holds position setpoint */
+            Eigen::Quaterniond q; /* holds desired yaw */
+
+            q = Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()) // roll
+                * Eigen::AngleAxisd(0.0,  Eigen::Vector3d::UnitY()) // pitch
+                * Eigen::AngleAxisd(req->yaw, Eigen::Vector3d::UnitZ()); // yaw
+
+            /* set position setpoint */
+            sp.translation() = current_local_pos + enu_offset;
+            /* set desired orientation */
+            sp.linear() = q.toRotationMatrix();
+
+            /* Only send if current gps is updated, to avoid divergence */
+            if ( (req->header.stamp.toNSec() / 1000000) > old_gps_stamp){
+                old_gps_stamp = req->header.stamp.toNSec() / 1000000;
+                /* send setpoint */
+                send_position_target(req->header.stamp, sp);
+            }
+            else {
+                ROS_WARN_THROTTLE_NAMED(10,"spgp", "SPG: sp not sent.");
+            }
+
+
+        }
+
+        /* current GPS coordinates*/
+        void gps_cb(const sensor_msgs::NavSatFix::ConstPtr &msg){
+                current_gps = {msg->latitude, msg->longitude, msg->altitude};
+        }
+
+        /* current local position in ENU */
+       void local_cb(const geometry_msgs::PoseStamped::ConstPtr &msg){
+                current_local_pos = {msg->pose.position.x, msg->pose.position.y, msg->pose.position.z};
+        }
+
 	bool set_mav_frame_cb(mavros_msgs::SetMavFrame::Request &req, mavros_msgs::SetMavFrame::Response &res)
 	{
 		mav_frame = static_cast<MAV_FRAME>(req.mav_frame);