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