Fix Quaternion toolbox, issue #587

facontidavide · Jan 29, 2022 · ef99a17 · ef99a17
1 parent 3959575
commit ef99a17
Show file tree

Hide file tree

Showing 2 changed files with 32 additions and 22 deletions.
diff --git a/plotjuggler_plugins/ToolboxQuaternion/toolbox_quaternion.cpp b/plotjuggler_plugins/ToolboxQuaternion/toolbox_quaternion.cpp
@@ -155,23 +155,39 @@ void ToolboxQuaternion::autoFill(QString prefix)
   }
 }
 
-void ToolboxQuaternion::generateRPY(GenerateType type)
+bool ToolboxQuaternion::generateRPY(GenerateType type)
 {
   using namespace PJ;
 
   bool wrap = ui->checkBoxUnwrap->isChecked();
   double unit_scale = ui->radioButtonDegrees->isChecked() ? (180.0 / M_PI) : 1.0;
   auto transform = std::make_shared<QuaternionToRollPitchYaw>();
 
-  auto src_data = getSrcData();
+  std::vector<const PlotData*> src_data;
+  {
+    for(QLineEdit* line: {ui->lineEditX, ui->lineEditY, ui->lineEditZ, ui->lineEditW })
+    {
+      auto it = _plot_data->numeric.find( line->text().toStdString() );
+      if( it == _plot_data->numeric.end() )
+      {
+        return false;
+      }
+      src_data.push_back(&it->second);
+    }
+  }
 
   std::string prefix = ui->lineEditOut->text().toStdString();
 
-  PlotData data_roll(prefix + "roll", {});
-  PlotData data_pitch(prefix + "pitch", {});
-  PlotData data_yaw(prefix + "yaw", {});
-  std::vector<PlotData*> dst_vector = { &data_roll, &data_pitch, &data_yaw };
+  // remove previous cruves bvefore creating new one
+  _plot_widget->removeAllCurves();
+
+  _preview_data_roll.reset( new PlotData(prefix + "roll", {}) );
+  _preview_data_pitch.reset( new PlotData(prefix + "pitch", {}) );
+  _preview_data_yaw.reset( new PlotData(prefix + "yaw", {}) );
 
+  std::vector<PlotData*> dst_vector = { _preview_data_roll.get(),
+                                        _preview_data_pitch.get(),
+                                        _preview_data_yaw.get() };
   if (type == SAVE)
   {
     dst_vector[0] = &_plot_data->getOrCreateNumeric(prefix + "roll", {});
@@ -188,7 +204,7 @@ void ToolboxQuaternion::generateRPY(GenerateType type)
   if (type == PREVIEW)
   {
     _plot_widget->removeAllCurves();
-    for (const auto& dst_data : dst_vector)
+    for (auto dst_data : dst_vector)
     {
       _plot_widget->addCurve(dst_data->plotName(), *dst_data);
     }
@@ -203,23 +219,13 @@ void ToolboxQuaternion::generateRPY(GenerateType type)
     emit plotCreated(prefix + "pitch");
     emit plotCreated(prefix + "yaw");
   }
-}
-
-std::vector<const PlotData*> ToolboxQuaternion::getSrcData()
-{
-  PlotData& data_x = _plot_data->getOrCreateNumeric(ui->lineEditX->text().toStdString());
-  PlotData& data_y = _plot_data->getOrCreateNumeric(ui->lineEditY->text().toStdString());
-  PlotData& data_z = _plot_data->getOrCreateNumeric(ui->lineEditZ->text().toStdString());
-  PlotData& data_w = _plot_data->getOrCreateNumeric(ui->lineEditW->text().toStdString());
-
-  return { &data_x, &data_y, &data_z, &data_w };
+  return true;
 }
 
 void ToolboxQuaternion::on_pushButtonSave_clicked()
 {
   generateRPY(SAVE);
 
-  ui->pushButtonSave->setEnabled(false);
   ui->lineEditX->setText({});
   ui->lineEditY->setText({});
   ui->lineEditZ->setText({});
@@ -236,9 +242,11 @@ void ToolboxQuaternion::onParametersChanged()
       ui->lineEditZ->text().isEmpty() || ui->lineEditW->text().isEmpty() ||
       ui->lineEditOut->text().isEmpty())
   {
+    ui->pushButtonSave->setEnabled(false);
     return;
   }
-  generateRPY(PREVIEW);
+  bool valid = generateRPY(PREVIEW);
+  ui->pushButtonSave->setEnabled(valid);
 }
 
 void ToolboxQuaternion::onClosed()

diff --git a/plotjuggler_plugins/ToolboxQuaternion/toolbox_quaternion.h b/plotjuggler_plugins/ToolboxQuaternion/toolbox_quaternion.h
@@ -59,13 +59,15 @@ private slots:
 
   PJ::TransformsMap* _transforms = nullptr;
 
+  std::unique_ptr<PlotData> _preview_data_roll;
+  std::unique_ptr<PlotData> _preview_data_pitch;
+  std::unique_ptr<PlotData> _preview_data_yaw;
+
   enum GenerateType
   {
     PREVIEW,
     SAVE
   };
 
-  void generateRPY(GenerateType type);
-
-  std::vector<const PlotData*> getSrcData();
+  bool generateRPY(GenerateType type);
 };