From 47ce78fcf1ec02f7a56fc18697647bf284828b84 Mon Sep 17 00:00:00 2001 From: Karthik Desai Date: Tue, 5 May 2020 16:25:52 +0200 Subject: [PATCH] Added Rally point plugin --- mavros_extras/CMakeLists.txt | 1 + mavros_extras/mavros_plugins.xml | 3 + mavros_extras/src/plugins/rally_point.cpp | 113 ++++++++++++++++++++++ mavros_msgs/CMakeLists.txt | 3 + mavros_msgs/msg/RallyPoint.msg | 17 ++++ mavros_msgs/srv/RallyPointGet.srv | 5 + mavros_msgs/srv/RallyPointSet.srv | 3 + 7 files changed, 145 insertions(+) create mode 100644 mavros_extras/src/plugins/rally_point.cpp create mode 100644 mavros_msgs/msg/RallyPoint.msg create mode 100644 mavros_msgs/srv/RallyPointGet.srv create mode 100644 mavros_msgs/srv/RallyPointSet.srv diff --git a/mavros_extras/CMakeLists.txt b/mavros_extras/CMakeLists.txt index ad129916e..4da573529 100644 --- a/mavros_extras/CMakeLists.txt +++ b/mavros_extras/CMakeLists.txt @@ -85,6 +85,7 @@ add_library(mavros_extras src/plugins/odom.cpp src/plugins/play_tune.cpp src/plugins/px4flow.cpp + src/plugins/rally_point.cpp src/plugins/rangefinder.cpp src/plugins/trajectory.cpp src/plugins/vibration.cpp diff --git a/mavros_extras/mavros_plugins.xml b/mavros_extras/mavros_plugins.xml index 4eac23db7..eec851395 100644 --- a/mavros_extras/mavros_plugins.xml +++ b/mavros_extras/mavros_plugins.xml @@ -54,6 +54,9 @@ Publish OPTICAL_FLOW message data from FCU or PX4Flow module. + + Ardupilot specific. Request rally point and set rally points. + Publish RANGEFINDER message data from FCU sensors in companion computer. diff --git a/mavros_extras/src/plugins/rally_point.cpp b/mavros_extras/src/plugins/rally_point.cpp new file mode 100644 index 000000000..4dab39c79 --- /dev/null +++ b/mavros_extras/src/plugins/rally_point.cpp @@ -0,0 +1,113 @@ +/** + * @brief Rally Point plugin + * @file rally_point.cpp + * @author Karthik Desai + * + * @addtogroup plugin + * @{ + */ +/* + * Copyright 2020 Karthik Desai + * + * This file is part of the mavros package and subject to the license terms + * in the top-level LICENSE file of the mavros repository. + * https://github.com/mavlink/mavros/tree/master/LICENSE.md + */ + +#include + +#include +#include + +#include +#include +#include + +namespace mavros { +namespace extra_plugins { + +class RallyPointPlugin : public plugin::PluginBase { +public: + RallyPointPlugin(): + nh("~rally_point") {} + void initialize(UAS& uas) override + { + PluginBase::initialize(uas); + + rally_point_pub = nh.advertise("point", 5); + + rally_point_request_srv = nh.advertiseService("get", + &RallyPointPlugin::rally_point_request_cb, this); + rally_point_set_srv = nh.advertiseService("set", + &RallyPointPlugin::rally_point_set_cb, this); + } + + Subscriptions get_subscriptions() override + { + return { + make_handler(&RallyPointPlugin::handle_rally_point), + }; + } +private: + ros::NodeHandle nh; + ros::Publisher rally_point_pub; + ros::ServiceServer rally_point_request_srv, rally_point_set_srv; + + void handle_rally_point(const mavlink::mavlink_message_t*, mavlink::ardupilotmega::msg::RALLY_POINT& rp) + { + auto msg = boost::make_shared(); + msg->header.stamp = ros::Time::now(); + msg->idx = rp.idx; + msg->count = rp.count; + msg->latitude = rp.lat / 1e7; + msg->longitude = rp.lng / 1e7; + msg->altitude = rp.alt; + msg->break_altitude = rp.break_alt; + msg->land_direction = rp.land_dir; + msg->flags = rp.flags; + + rally_point_pub.publish(msg); + } + + bool rally_point_request_cb(mavros_msgs::RallyPointGet::Request &req, + mavros_msgs::RallyPointGet::Response &resp) + { + mavlink::ardupilotmega::msg::RALLY_FETCH_POINT msg; + m_uas->msg_set_target(msg); + msg.idx = req.idx; + resp.success = true; + try { + UAS_FCU(m_uas)->send_message(msg); + } catch (std::length_error&) { + resp.success = false; + } + return true; + } + + bool rally_point_set_cb(mavros_msgs::RallyPointSet::Request &req, + mavros_msgs::RallyPointSet::Response &resp) + { + mavlink::ardupilotmega::msg::RALLY_POINT msg; + m_uas->msg_set_target(msg); + msg.idx = req.rally.idx; + msg.count = req.rally.count; + msg.lat = req.rally.latitude * 1e7; + msg.lng = req.rally.longitude * 1e7; + msg.alt = req.rally.altitude; + msg.break_alt = req.rally.break_altitude; + msg.land_dir = req.rally.land_direction; + msg.flags = req.rally.flags; + resp.success = true; + try { + UAS_FCU(m_uas)->send_message(msg); + } catch (std::length_error&) { + resp.success = false; + } + return true; + } +}; +} // namespace extra_plugins +} // namespace mavros + +#include +PLUGINLIB_EXPORT_CLASS(mavros::extra_plugins::RallyPointPlugin, mavros::plugin::PluginBase) \ No newline at end of file diff --git a/mavros_msgs/CMakeLists.txt b/mavros_msgs/CMakeLists.txt index 049948cb6..e88753654 100644 --- a/mavros_msgs/CMakeLists.txt +++ b/mavros_msgs/CMakeLists.txt @@ -49,6 +49,7 @@ add_message_files( ParamValue.msg PlayTuneV2.msg PositionTarget.msg + RallyPoint.msg RCIn.msg RCOut.msg RTCM.msg @@ -100,6 +101,8 @@ add_service_files( ParamPull.srv ParamPush.srv ParamSet.srv + RallyPointGet.srv + RallyPointSet.srv SetMavFrame.srv SetMode.srv StreamRate.srv diff --git a/mavros_msgs/msg/RallyPoint.msg b/mavros_msgs/msg/RallyPoint.msg new file mode 100644 index 000000000..1ecd467d2 --- /dev/null +++ b/mavros_msgs/msg/RallyPoint.msg @@ -0,0 +1,17 @@ +# Rally Point message based on +# https://mavlink.io/en/messages/ardupilotmega.html#RALLY_POINT + +# Flags +uint8 FAVORABLE_WIND = 1 +uint8 LAND_IMMEDIATELY = 2 + +std_msgs/Header header + +uint8 idx +uint8 count +float32 latitude +float32 longitude +int16 altitude +int16 break_altitude +uint16 land_direction +uint8 flags \ No newline at end of file diff --git a/mavros_msgs/srv/RallyPointGet.srv b/mavros_msgs/srv/RallyPointGet.srv new file mode 100644 index 000000000..815a24eae --- /dev/null +++ b/mavros_msgs/srv/RallyPointGet.srv @@ -0,0 +1,5 @@ +# Request a rally point msg + +uint8 idx +--- +bool success \ No newline at end of file diff --git a/mavros_msgs/srv/RallyPointSet.srv b/mavros_msgs/srv/RallyPointSet.srv new file mode 100644 index 000000000..62503db37 --- /dev/null +++ b/mavros_msgs/srv/RallyPointSet.srv @@ -0,0 +1,3 @@ +mavros_msgs/RallyPoint rally +--- +bool success \ No newline at end of file