From 7c3fcb563ceb2efd43fdc862d98e722294ecccbc Mon Sep 17 00:00:00 2001 From: Sanket Sharma Date: Tue, 19 Jul 2022 02:41:01 +0530 Subject: [PATCH] Update guided_target.cpp Update mavros_plugins.xml Update CMakeLists.txt Added offboard_position.cpp Update apm_config.yaml Update offboard_position.cpp Update offboard_position.cpp Rename offboard_position.cpp to guided_target.cpp Update CMakeLists.txt Update mavros_plugins.xml Update apm_config.yaml --- mavros/launch/apm_config.yaml | 8 + mavros_extras/CMakeLists.txt | 1 + mavros_extras/mavros_plugins.xml | 3 + mavros_extras/src/plugins/guided_target.cpp | 192 ++++++++++++++++++++ 4 files changed, 204 insertions(+) create mode 100644 mavros_extras/src/plugins/guided_target.cpp diff --git a/mavros/launch/apm_config.yaml b/mavros/launch/apm_config.yaml index 2f1963f18..63eac7aa2 100644 --- a/mavros/launch/apm_config.yaml +++ b/mavros/launch/apm_config.yaml @@ -112,6 +112,14 @@ setpoint_position: child_frame_id: "target_position" rate_limit: 50.0 mav_frame: LOCAL_NED + +# guided_target +guided_target: + tf: + listen: false # enable tf listener (disable topic subscribers) + frame_id: "map" + child_frame_id: "target_position" + rate_limit: 50.0 # setpoint_velocity setpoint_velocity: diff --git a/mavros_extras/CMakeLists.txt b/mavros_extras/CMakeLists.txt index 51fa8eb1e..0b19ac443 100644 --- a/mavros_extras/CMakeLists.txt +++ b/mavros_extras/CMakeLists.txt @@ -96,6 +96,7 @@ add_library(mavros_extras src/plugins/vision_pose_estimate.cpp src/plugins/vision_speed_estimate.cpp src/plugins/wheel_odometry.cpp + src/plugins/guided_target.cpp ) add_dependencies(mavros_extras ${catkin_EXPORTED_TARGETS} diff --git a/mavros_extras/mavros_plugins.xml b/mavros_extras/mavros_plugins.xml index 7dd35831b..d82f76bbf 100644 --- a/mavros_extras/mavros_plugins.xml +++ b/mavros_extras/mavros_plugins.xml @@ -93,4 +93,7 @@ Send calibration status to ROS. + + Send guided target. + diff --git a/mavros_extras/src/plugins/guided_target.cpp b/mavros_extras/src/plugins/guided_target.cpp new file mode 100644 index 000000000..331ae02d2 --- /dev/null +++ b/mavros_extras/src/plugins/guided_target.cpp @@ -0,0 +1,192 @@ +#include +#include +#include + +#include + +#include +#include + +#include + +#include +#include +#include + +namespace mavros { +namespace extra_plugins { +/** + * @brief guided target plugin + * + * Send and receive setpoint positions from FCU controller. + */ +class GuidedTargetPlugin : public plugin::PluginBase, + private plugin::SetPositionTargetLocalNEDMixin, + private plugin::SetPositionTargetGlobalIntMixin, + private plugin::TF2ListenerMixin { +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + GuidedTargetPlugin() : PluginBase(), + sp_nh("~guided_target"), + spg_nh("~"), + tf_listen(false), + tf_rate(50.0), + is_map_init(false) + { } + + void initialize(UAS &uas_) override + { + PluginBase::initialize(uas_); + + // tf params + sp_nh.param("tf/listen", tf_listen, false); + sp_nh.param("tf/frame_id", tf_frame_id, "map"); + sp_nh.param("tf/child_frame_id", tf_child_frame_id, "target_position"); + sp_nh.param("tf/rate_limit", tf_rate, 50.0); + + // Publish targets received from FCU + setpointg_pub = spg_nh.advertise("/move_base_simple/goal", 10); + + + // Subscriber for global origin (aka map origin). + gp_origin_sub = spg_nh.subscribe("global_position/gp_origin", 10, &GuidedTargetPlugin::gp_origin_cb, this); + + + + + } + + Subscriptions get_subscriptions() override + { + //return { /* Rx disabled */ }; + + return { + make_handler(&GuidedTargetPlugin::handle_position_target_global_int) + }; + } + +private: + + + ros::NodeHandle sp_nh; + ros::NodeHandle spg_nh; //!< to get local position and gps coord which are not under sp_h() + + ros::Subscriber gp_origin_sub; //!< global origin LLA + + 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 {}; //!< oigin of map frame [lla] + Eigen::Vector3d ecef_origin {}; //!< geocentric origin [m] + + 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; + std::string tf_child_frame_id; + + bool tf_listen; + double tf_rate; + bool is_map_init; + + double arr[2] = {0, 0}; + + /* -*- mid-level helpers -*- */ + + /** + * @brief Send setpoint to FCU position controller. + * + * @warning Send only XYZ, Yaw. ENU frame. + */ + + + /** + * global origin in LLA + */ + void gp_origin_cb(const geographic_msgs::GeoPointStamped::ConstPtr &msg) + { + ecef_origin = {msg->position.latitude, msg->position.longitude, msg->position.altitude}; + /** + * @brief Conversion from ECEF (Earth-Centered, Earth-Fixed) to geodetic coordinates (LLA) + */ + GeographicLib::Geocentric earth(GeographicLib::Constants::WGS84_a(), GeographicLib::Constants::WGS84_f()); + try { + earth.Reverse(ecef_origin.x(), ecef_origin.y(), ecef_origin.z(), + map_origin.x(), map_origin.y(), map_origin.z()); + } + catch (const std::exception& e) { + ROS_WARN_STREAM("setpoint: Caught exception: " << e.what() << std::endl); + return; + } + is_map_init = true; + } + + + + + /* -*- rx handler -*- */ + + /** + * @brief handle POSITION_TARGET_GLOBAL_INT mavlink msg + * handles and publishes position target received from FCU + */ + + + + void handle_position_target_global_int(const mavlink::mavlink_message_t *msg, mavlink::common::msg::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", "PositionTargetGlobal 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 - ecef_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 */ + + if (pose->pose.position.x != arr[0] | pose->pose.position.y != arr[1]) { + setpointg_pub.publish(pose); + } + + arr[0] = pose->pose.position.x; + arr[1] = pose->pose.position.y; + } + + + +}; +} // namespace std_plugins +} // namespace mavros + +#include +PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::GuidedTargetPlugin, mavros::plugin::PluginBase)