From cfc72fed9732c3151b8923b8898fdd73e051ccb6 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Fri, 15 Feb 2019 16:19:43 +0900 Subject: [PATCH] setpoint_position: accept set-position-target-global-int messages --- mavros/src/plugins/setpoint_position.cpp | 76 +++++++++++++++++++++++- 1 file changed, 73 insertions(+), 3 deletions(-) diff --git a/mavros/src/plugins/setpoint_position.cpp b/mavros/src/plugins/setpoint_position.cpp index ebce51e91..9604c1817 100644 --- a/mavros/src/plugins/setpoint_position.cpp +++ b/mavros/src/plugins/setpoint_position.cpp @@ -19,9 +19,11 @@ #include #include +#include #include #include +#include #include //#include @@ -32,7 +34,7 @@ using mavlink::common::MAV_FRAME; /** * @brief Setpoint position plugin * - * Send setpoint positions to FCU controller. + * Send and receive setpoint positions from FCU controller. */ class SetpointPositionPlugin : public plugin::PluginBase, private plugin::SetPositionTargetLocalNEDMixin, @@ -44,7 +46,8 @@ class SetpointPositionPlugin : public plugin::PluginBase, sp_nh("~setpoint_position"), spg_nh("~"), tf_rate(50.0), - tf_listen(false) + tf_listen(false), + is_map_init(false) { } void initialize(UAS &uas_) @@ -71,6 +74,9 @@ class SetpointPositionPlugin : public plugin::PluginBase, 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); + + // subscriber for global origin (aka map origin) + gp_origin_sub = spg_nh.subscribe("global_position/gp_origin", 10, &SetpointPositionPlugin::gp_origin_cb, this); } mav_frame_srv = sp_nh.advertiseService("mav_frame", &SetpointPositionPlugin::set_mav_frame_cb, this); @@ -81,11 +87,16 @@ class SetpointPositionPlugin : public plugin::PluginBase, } else { mav_frame = utils::mav_frame_from_str(mav_frame_str); } + + // publish targets received from FCU + setpointg_pub = sp_nh.advertise("cmd_pos", 10); } Subscriptions get_subscriptions() { - return { /* Rx disabled */ }; + return { + make_handler(&SetpointPositionPlugin::handle_set_position_target_global_int), + }; } private: @@ -98,12 +109,15 @@ class SetpointPositionPlugin : public plugin::PluginBase, ros::Subscriber setpointg_sub; //!< GPS setpoint ros::Subscriber gps_sub; //!< current GPS ros::Subscriber local_sub; //!< current local ENU + ros::Subscriber gp_origin_sub; //!< global origin LLA ros::ServiceServer mav_frame_srv; + ros::Publisher setpointg_pub; //!< global position target from FCU /* Stores current gps state. */ //sensor_msgs::NavSatFix current_gps_msg; Eigen::Vector3d current_gps; //!< geodetic coordinates LLA Eigen::Vector3d current_local_pos; //!< Current local position in ENU + Eigen::Vector3d map_origin {}; //!< origin of map frame [lla] uint32_t old_gps_stamp = 0; //!< old time gps time stamp in [ms], to check if new gps msg is received std::string tf_frame_id; @@ -111,6 +125,7 @@ class SetpointPositionPlugin : public plugin::PluginBase, bool tf_listen; double tf_rate; + bool is_map_init; MAV_FRAME mav_frame; @@ -250,6 +265,15 @@ class SetpointPositionPlugin : public plugin::PluginBase, current_local_pos = ftf::to_eigen(msg->pose.position); } + /** + * global origin in LLA + */ + void gp_origin_cb(const geographic_msgs::GeoPointStamped::ConstPtr &msg) + { + map_origin = {msg->position.latitude, msg->position.longitude, msg->position.altitude}; + is_map_init = true; + } + bool set_mav_frame_cb(mavros_msgs::SetMavFrame::Request &req, mavros_msgs::SetMavFrame::Response &res) { mav_frame = static_cast(req.mav_frame); @@ -258,6 +282,52 @@ class SetpointPositionPlugin : public plugin::PluginBase, res.success = true; return true; } + + /* -*- rx handler -*- */ + + /** + * @brief handle SET_POSITION_TARGET_GLOBAL_INT mavlink msg + * handles and publishes position target received from FCU + */ + void handle_set_position_target_global_int(const mavlink::mavlink_message_t *msg, mavlink::common::msg::SET_POSITION_TARGET_GLOBAL_INT &position_target) + { + /* check if type_mask field ignores position*/ + if (position_target.type_mask & (mavros_msgs::GlobalPositionTarget::IGNORE_LATITUDE | mavros_msgs::GlobalPositionTarget::IGNORE_LONGITUDE) > 0) { + ROS_WARN_NAMED("setpoint", "lat and/or lon ignored"); + return; + } + + /* check origin has been set */ + if (!is_map_init) { + ROS_WARN_NAMED("setpoint", "SetPositionTargetGlobal failed because no origin"); + } + + /* convert lat/lon target to ECEF */ + Eigen::Vector3d pos_target_ecef {}; //!< local ECEF coordinates on map frame [m] + GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f()); + try { + earth.Forward(position_target.lat_int / 1E7, position_target.lon_int / 1E7, position_target.alt / 1E3, + pos_target_ecef.x(), pos_target_ecef.y(), pos_target_ecef.z()); + } + catch (const std::exception& e) { + ROS_WARN_STREAM("setpoint: Caught exception: " << e.what() << std::endl); + return; + } + + /* create position target PoseStamped message */ + auto pose = boost::make_shared(); + pose->header = m_uas->synchronized_header("map", position_target.time_boot_ms); + pose->pose.orientation.w = 1; // unit quaternion with no rotation + + /* convert ECEF target to ENU */ + const Eigen::Vector3d local_ecef = pos_target_ecef - map_origin; + tf::pointEigenToMsg(ftf::transform_frame_ecef_enu(local_ecef, map_origin), pose->pose.position); + pose->pose.position.z = 0; // force z-axis to zero + + /* publish target */ + setpointg_pub.publish(pose); + } + }; } // namespace std_plugins } // namespace mavros