ros2: add flight stack combo box to planning panel

- The commands used by engage planner and disengage planner buttons are flight stack specific

Signed-off-by: Rhys Mainwaring <[email protected]>

mav_planning_rviz/include/mav_planning_rviz/planning_panel.h

@@ -16,12 +16,14 @@
 #include "mav_planning_rviz/pose_widget.h"
 #endif
 
-enum PLANNER_STATE { HOLD = 1, NAVIGATE = 2, ROLLOUT = 3, ABORT = 4, RETURN = 5 };
-
 class QLineEdit;
 class QCheckBox;
+class QComboBox;
 namespace mav_planning_rviz {
 
+enum PLANNER_STATE { HOLD = 1, NAVIGATE = 2, ROLLOUT = 3, ABORT = 4, RETURN = 5 };
+enum FLIGHT_STACK { FLIGHT_STACK_NONE = 0, PX4 = 1, ARDUPILOT = 2 };
+
 class PlanningPanel : public rviz_common::Panel {
   // This class uses Qt slots and is a subclass of QObject, so it needs
   // the Q_OBJECT macro.
@@ -57,6 +59,7 @@ class PlanningPanel : public rviz_common::Panel {
   // Next come a couple of public Qt slots.
  public Q_SLOTS:
   void updatePlannerName();
+  void updateFlightStack();
   void updatePlanningBudget();
   void setPlannerName();
   void startEditing(const std::string& id);
@@ -107,6 +110,7 @@ class PlanningPanel : public rviz_common::Panel {
   // QT stuff:
   QLineEdit* namespace_editor_;
   QLineEdit* planner_name_editor_;
+  QComboBox* flight_stack_combobox_;
   QLineEdit* odometry_topic_editor_;
   QLineEdit* planning_budget_editor_;
   QCheckBox* terrain_align_checkbox_;
@@ -141,6 +145,10 @@ class PlanningPanel : public rviz_common::Panel {
 
   // Other state:
   std::string currently_editing_;
+
+  // Select flight stack (affects offboard/guided and RTL commands)
+  QStringList flight_stack_names_ = {"px4", "ardupilot"};
+  FLIGHT_STACK flight_stack_{PX4};
 };
 
 }  // end namespace mav_planning_rviz

mav_planning_rviz/src/planning_panel.cpp

@@ -3,6 +3,7 @@
 #include <stdio.h>
 
 #include <QCheckBox>
+#include <QComboBox>
 #include <QGridLayout>
 #include <QHBoxLayout>
 #include <QLabel>
@@ -33,6 +34,30 @@ using namespace std::chrono_literals;
 
 namespace mav_planning_rviz {
 
+// utility to convert flight stack string to enum.
+FLIGHT_STACK to_flight_stack(const std::string& str) {
+  FLIGHT_STACK flight_stack = FLIGHT_STACK_NONE;
+  if (str == "px4") {
+    flight_stack = PX4;
+  } else if (str == "ardupilot") {
+    flight_stack = ARDUPILOT;
+  }
+  return flight_stack;
+}
+
+// utility to convert flight stack enum to string.
+std::string to_string(FLIGHT_STACK flight_stack) {
+  switch (flight_stack) {
+    case PX4:
+      return "px4";
+    case ARDUPILOT:
+      return "ardupilot";
+    case FLIGHT_STACK_NONE:
+    default:
+      return "none";
+  }
+}
+
 PlanningPanel::PlanningPanel(QWidget* parent)
     : rviz_common::Panel(parent),
       node_(std::make_shared<rclcpp::Node>("mav_planning_rviz")),
@@ -161,8 +186,14 @@ QGroupBox* PlanningPanel::createTerrainLoaderGroup() {
   load_terrain_button_ = new QPushButton("Load Terrain");
   service_layout->addWidget(load_terrain_button_, 0, 3, 1, 1);
 
+  service_layout->addWidget(new QLabel("Flight Stack:"), 0, 4, 1, 1);
+  flight_stack_combobox_ = new QComboBox;
+  flight_stack_combobox_->addItems(flight_stack_names_);
+  service_layout->addWidget(flight_stack_combobox_, 0, 5, 1, 1);
+
   connect(planner_name_editor_, SIGNAL(editingFinished()), this, SLOT(updatePlannerName()));
   connect(load_terrain_button_, SIGNAL(released()), this, SLOT(setPlannerName()));
+  connect(flight_stack_combobox_, SIGNAL(currentIndexChanged(int)), this, SLOT(updateFlightStack()));
 
   groupBox->setLayout(service_layout);
 
@@ -210,6 +241,16 @@ void PlanningPanel::updatePlannerName() {
   }
 }
 
+void PlanningPanel::updateFlightStack() {
+  std::string flight_stack_name = flight_stack_combobox_->currentText().toStdString();
+  std::cout << "Using flight stack: " << flight_stack_name << std::endl;
+  FLIGHT_STACK new_flight_stack = to_flight_stack(flight_stack_name);
+  if (new_flight_stack != flight_stack_) {
+    flight_stack_ = new_flight_stack;
+    Q_EMIT configChanged();
+  }
+}
+
 // Set the topic name we are publishing to.
 void PlanningPanel::setPlannerName() {
   std::cout << "[PlanningPanel] Loading new terrain:" << planner_name_.toStdString() << std::endl;
@@ -335,19 +376,24 @@ void PlanningPanel::save(rviz_common::Config config) const {
   config.mapSetValue("planner_name", planner_name_);
   config.mapSetValue("planning_budget", planning_budget_value_);
   config.mapSetValue("odometry_topic", odometry_topic_);
+  config.mapSetValue("flight_stack", QString::fromStdString(to_string(flight_stack_)));
 }
 
 // Load all configuration data for this panel from the given Config object.
 void PlanningPanel::load(const rviz_common::Config& config) {
   rviz_common::Panel::load(config);
-  QString topic;
-  QString ns;
   if (config.mapGetString("planner_name", &planner_name_)) {
     planner_name_editor_->setText(planner_name_);
   }
   if (config.mapGetString("planning_budget", &planning_budget_value_)) {
     planning_budget_editor_->setText(planning_budget_value_);
   }
+
+  QString flight_stack_name;
+  if (config.mapGetString("flight_stack", &flight_stack_name)) {
+    int index = flight_stack_combobox_->findText(flight_stack_name);
+    flight_stack_combobox_->setCurrentIndex(index);
+  }
 }
 
 void PlanningPanel::updateInteractiveMarkerPose(const mav_msgs::EigenTrajectoryPoint& pose) {
@@ -379,10 +425,19 @@ void PlanningPanel::callPlannerService() {
     }
 
     auto req = std::make_shared<mavros_msgs::srv::SetMode::Request>();
-    req->custom_mode = "OFFBOARD";
-    //! @todo(srmainwaring) for AP custom mode is "GUIDED".
-    // req->custom_mode = "GUIDED";
-
+    req->custom_mode = "GUIDED";
+    switch (flight_stack_) {
+      case PX4:
+        req->custom_mode = "OFFBOARD";
+        break;
+      case ARDUPILOT:
+        req->custom_mode = "GUIDED";
+        break;
+      case FLIGHT_STACK_NONE:
+      default:
+        req->custom_mode = "NONE";
+        break;
+    }
     auto result = client->async_send_request(req);
 
     //! @todo(srmainwaring) prevent race condition with async service calls
@@ -443,8 +498,18 @@ void PlanningPanel::publishWaypoint() {
     }
 
     auto req = std::make_shared<mavros_msgs::srv::SetMode::Request>();
-    req->custom_mode = "AUTO.RTL";
-
+    switch (flight_stack_) {
+      case PX4:
+        req->custom_mode = "AUTO.RTL";
+        break;
+      case ARDUPILOT:
+        req->custom_mode = "RTL";
+        break;
+      case FLIGHT_STACK_NONE:
+      default:
+        req->custom_mode = "NONE";
+        break;
+    }
     auto result = client->async_send_request(req);
 
     //! @todo(srmainwaring) prevent race condition with async service calls