Fix rviz panel thread race condition

nav2_rviz_plugins/include/nav2_rviz_plugins/nav2_panel.hpp

@@ -35,6 +35,8 @@ class QPushButton;
 namespace nav2_rviz_plugins
 {
 
+class InitialThread;
+
 /// Panel to interface to the nav2 stack
 class Nav2Panel : public rviz_common::Panel
 {
@@ -51,6 +53,7 @@ public:
   void save(rviz_common::Config config) const override;
 
 private Q_SLOTS:
+  void startThread();
   void onStartup();
   void onShutdown();
   void onCancel();
@@ -87,6 +90,7 @@ private:
   QPushButton * pause_resume_button_{nullptr};
 
   QStateMachine state_machine_;
+  InitialThread * initial_thread_;
 
   QState * pre_initial_{nullptr};
   QState * initial_{nullptr};

nav2_rviz_plugins/src/nav2_panel.cpp

@@ -136,15 +136,15 @@ Nav2Panel::Nav2Panel(QWidget * parent)
   runningTransition->setTargetState(idle_);
   running_->addTransition(runningTransition);
 
-  InitialThread * initialThread = new InitialThread(client_);
-  connect(initialThread, &InitialThread::finished, initialThread, &QObject::deleteLater);
+  initial_thread_ = new InitialThread(client_);
+  connect(initial_thread_, &InitialThread::finished, initial_thread_, &QObject::deleteLater);
 
-  QSignalTransition * activeSignal = new QSignalTransition(initialThread,
+  QSignalTransition * activeSignal = new QSignalTransition(initial_thread_,
       &InitialThread::activeSystem);
   activeSignal->setTargetState(idle_);
   pre_initial_->addTransition(activeSignal);
 
-  QSignalTransition * inactiveSignal = new QSignalTransition(initialThread,
+  QSignalTransition * inactiveSignal = new QSignalTransition(initial_thread_,
       &InitialThread::inactiveSystem);
   inactiveSignal->setTargetState(initial_);
   pre_initial_->addTransition(inactiveSignal);
@@ -161,6 +161,9 @@ Nav2Panel::Nav2Panel(QWidget * parent)
   state_machine_.setInitialState(pre_initial_);
   state_machine_.start();
 
+  // delay starting initial thread until state machine has started or a race occurs
+  QObject::connect(&state_machine_, SIGNAL(started()), this, SLOT(startThread()));
+
   // Lay out the items in the panel
   QVBoxLayout * main_layout = new QVBoxLayout;
   main_layout->addWidget(pause_resume_button_);
@@ -178,8 +181,6 @@ Nav2Panel::Nav2Panel(QWidget * parent)
 
   QObject::connect(&GoalUpdater, SIGNAL(updateGoal(double,double,double,QString)),  // NOLINT
     this, SLOT(onNewGoal(double,double,double,QString)));  // NOLINT
-
-  initialThread->start();
 }
 
 Nav2Panel::~Nav2Panel()
@@ -192,6 +193,13 @@ Nav2Panel::onInitialize()
   auto node = getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
 }
 
+void
+Nav2Panel::startThread()
+{
+  // start initial thread now that state machine is started
+  initial_thread_->start();
+}
+
 void
 Nav2Panel::onPause()
 {