diff --git a/scenario_execution_rviz/README.md b/scenario_execution_rviz/README.md index 1430371f..9b5ba208 100644 --- a/scenario_execution_rviz/README.md +++ b/scenario_execution_rviz/README.md @@ -9,9 +9,7 @@ The following image shows a snapshot of the rviz plugin during a run of the [exa ![tree_example.png](../docs/images/scenario_view_example.png) -### Known Issues - -The Scenario View panel can not display the scenario's behavior tree if initialized while the robot navigation is already running. +The plugin subscribes to `/scenario_execution/snapshots` by default. Double-click on the header to modify the topic. ### Icon Licence diff --git a/scenario_execution_rviz/src/scenario_view.cpp b/scenario_execution_rviz/src/scenario_view.cpp index 967fc830..835aebe3 100644 --- a/scenario_execution_rviz/src/scenario_view.cpp +++ b/scenario_execution_rviz/src/scenario_view.cpp @@ -16,12 +16,17 @@ #include "scenario_view.h" #include "indicator_widget.h" - +#include +#include using std::placeholders::_1; namespace scenario_execution_rviz { -ScenarioView::ScenarioView(QWidget *parent) : rviz_common::Panel(parent) { +ScenarioView::ScenarioView(QWidget *parent) + : rviz_common::Panel(parent), mInitTimer(this) { + mInitTimer.setSingleShot(true); + connect(&mInitTimer, &QTimer::timeout, this, &ScenarioView::setupConnection); + QVBoxLayout *layout = new QVBoxLayout; QFormLayout *formLayout = new QFormLayout; @@ -51,14 +56,58 @@ ScenarioView::ScenarioView(QWidget *parent) : rviz_common::Panel(parent) { SLOT(handleItemCollapsed(QTreeWidgetItem *))); connect(mScenarioView, SIGNAL(itemExpanded(QTreeWidgetItem *)), this, SLOT(handleItemExpanded(QTreeWidgetItem *))); + connect(mScenarioView->header(), SIGNAL(sectionDoubleClicked(int)), this, + SLOT(onHeaderDoubleClicked(int))); +} + +void ScenarioView::load(const rviz_common::Config &config) { + rviz_common::Panel::load(config); + QString topic; + if (config.mapGetString("snapshot_topic", &topic)) { + + if (mSnapshotTopic != topic) { + mInitTimer.stop(); // stop init trigger + mSnapshotTopic = topic; + setupConnection(); + } + } +} + +void ScenarioView::save(rviz_common::Config config) const { + rviz_common::Panel::save(config); + config.mapSetValue("snapshot_topic", mSnapshotTopic); +} + +void ScenarioView::onHeaderDoubleClicked(int idx) { + (void)idx; + bool ok; + QString text = QInputDialog::getText(this, "Configuration", "Snapshot Topic", + QLineEdit::Normal, mSnapshotTopic, &ok); + if (ok && !text.isEmpty() && (mSnapshotTopic != text)) { + mSnapshotTopic = text; + setupConnection(); + Q_EMIT configChanged(); + } } void ScenarioView::onInitialize() { _node = getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + mSnapshotTopic = "/scenario_execution/snapshots"; + mInitTimer.start(250); // only executed if no config-load gets triggered. +} + +void ScenarioView::setupConnection() { + if (mSnapshotTopic.isEmpty()) { + return; + } + if (mBehaviorTreeSubscriber) { + mBehaviorTreeSubscriber.reset(); + } + qDebug() << "Subscribing to" << mSnapshotTopic; mBehaviorTreeSubscriber = _node->create_subscription( - "/scenario_execution/snapshots", 10, + mSnapshotTopic.toStdString().c_str(), 10, std::bind(&ScenarioView::behaviorTreeChanged, this, _1)); } diff --git a/scenario_execution_rviz/src/scenario_view.h b/scenario_execution_rviz/src/scenario_view.h index 9898a1fb..9d5ca1cf 100644 --- a/scenario_execution_rviz/src/scenario_view.h +++ b/scenario_execution_rviz/src/scenario_view.h @@ -64,9 +64,17 @@ class ScenarioView : public rviz_common::Panel { public: ScenarioView(QWidget *parent = 0); + virtual void load(const rviz_common::Config &config) override; + virtual void save(rviz_common::Config config) const override; + +Q_SIGNALS: + void doubleClickedA(int index); + protected Q_SLOTS: void handleItemCollapsed(QTreeWidgetItem *collapsedItem); void handleItemExpanded(QTreeWidgetItem *expandedItem); + void onHeaderDoubleClicked(int idx); + void setupConnection(); protected: virtual void onInitialize() override; @@ -93,6 +101,8 @@ protected Q_SLOTS: TreeModel *mTreeModel; bool treeWidgetBuilt = false; QMap *collapsedStates; + QString mSnapshotTopic; + QTimer mInitTimer; // icons QIcon runningIcon = QIcon(":/icons/chevron-right-o.png");