Publish behavior tree by default, update clients (#111)

.github/workflows/test_build.yml

@@ -236,7 +236,7 @@ jobs:
           export IGN_PARTITION=${HOSTNAME}:${GITHUB_RUN_ID}
           sed -i 's/60s/600s/g' examples/example_nav2/example_nav2.osc
           # shellcheck disable=SC1083
-          scenario_batch_execution -i examples/example_nav2/ -o test_example_nav2 -- ros2 launch tb4_sim_scenario sim_nav_scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR}
+          scenario_batch_execution -i examples/example_nav2/ -o test_example_nav2 -- ros2 launch tb4_sim_scenario sim_nav_scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR} headless:=True
       - name: Upload result
         uses: actions/upload-artifact@ef09cdac3e2d3e60d8ccadda691f4f1cec5035cb
         if: always()
@@ -269,7 +269,7 @@ jobs:
           export IGN_PARTITION=${HOSTNAME}:${GITHUB_RUN_ID}
           sed -i 's/120s/600s/g' examples/example_simulation/scenarios/example_simulation.osc
           # shellcheck disable=SC1083
-          scenario_batch_execution -i examples/example_simulation/scenarios/ -o test_example_simulation -- ros2 launch tb4_sim_scenario sim_nav_scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR}
+          scenario_batch_execution -i examples/example_simulation/scenarios/ -o test_example_simulation -- ros2 launch tb4_sim_scenario sim_nav_scenario_launch.py scenario:={SCENARIO} output_dir:={OUTPUT_DIR} headless:=True
       - name: Upload result
         uses: actions/upload-artifact@ef09cdac3e2d3e60d8ccadda691f4f1cec5035cb
         if: always()
@@ -302,7 +302,7 @@ jobs:
           export IGN_PARTITION=${HOSTNAME}:${GITHUB_RUN_ID}
           sed -i 's/240s/900s/g' examples/example_multi_robot/example_multi_robot.osc
           # shellcheck disable=SC1083
-          scenario_batch_execution -i examples/example_multi_robot/ -o test_example_multirobot -- ros2 launch tb4_sim_scenario sim_nav_scenario_launch.py scenario:={SCENARIO} yaw:=3.14 output_dir:={OUTPUT_DIR}
+          scenario_batch_execution -i examples/example_multi_robot/ -o test_example_multirobot -- ros2 launch tb4_sim_scenario sim_nav_scenario_launch.py scenario:={SCENARIO} yaw:=3.14 output_dir:={OUTPUT_DIR} headless:=True
       - name: Upload result
         uses: actions/upload-artifact@ef09cdac3e2d3e60d8ccadda691f4f1cec5035cb
         if: always()

Makefile

@@ -2,8 +2,7 @@ file_finder = find . -type f $(1) -not \( -path './venv/*' -o -path './build/*'
 
 PY_FILES = $(call file_finder,-name "*.py")
 CPP_FILES = $(call file_finder,-name "*.cpp")
-H_FILES = $(call file_finder,-name "*.cpp")
-C_FILES = $(call file_finder,-name "*.cpp")
+H_FILES = $(call file_finder,-name "*.h")
 
 LINKCHECKDIR  = build/linkcheck
 
@@ -13,7 +12,6 @@ format:
 	$(PY_FILES) | xargs autopep8 --in-place --max-line-length=140
 	$(CPP_FILES) | xargs clang-format -i
 	$(H_FILES) | xargs clang-format -i
-	$(C_FILES) | xargs clang-format -i
 
 check_format:
 	$(PY_FILES) | xargs autopep8 --diff --max-line-length=140 --exit-code

docs/libraries.rst

@@ -698,7 +698,9 @@ Wait for specific output in ROS log (i.e. ``/rosout`` topic). If any of the entr
 ``record_bag()``
 """"""""""""""""
 
-Record a ROS bag, stored in directory ``output_dir`` defined by command-line parameter (default: '.')
+Record a ROS bag, stored in directory ``output_dir`` defined by command-line parameter (default: '.').
+
+A common topic to record is ``/scenario_execution/snapshots`` which publishes changes within the behavior tree. When replaying the bag-file, this allows to visualize the current state of the scenario in RViz, using the ``scenario_execution_rviz`` plugin.
 
 .. list-table:: 
    :widths: 15 15 5 65

scenario_execution/scenario_execution/scenario_execution_base.py

@@ -159,6 +159,7 @@ def setup(self, scenario: py_trees.behaviour.Behaviour, **kwargs) -> bool:
             input_dir = os.path.dirname(self.scenario_file)
         self.behaviour_tree.setup(timeout=self.setup_timeout, logger=self.logger,
                                   input_dir=input_dir, output_dir=self.output_dir, **kwargs)
+        self.post_setup()
 
     def setup_behaviour_tree(self, tree):
         """
@@ -175,6 +176,9 @@ def setup_behaviour_tree(self, tree):
         """
         return py_trees.trees.BehaviourTree(tree)
 
+    def post_setup(self):
+        pass
+
     def parse(self):  # pylint: disable=too-many-return-statements
         """
         Parse the OpenScenario2 file

scenario_execution_ros/launch/scenario_launch.py

@@ -73,9 +73,7 @@ def generate_launch_description():
             executable='scenario_status_node',
             name='scenario_status_node',
             parameters=[{
-                'bt_snapshot_topic': '/bt_snapshot',
                 'scenario_status_topic': '/scenario_status',
-                'snapshot_srv_name': '/scenario_execution/snapshot_streams/open',
 
             }],
             output='screen'

scenario_execution_ros/scenario_execution_ros/scenario_execution_ros.py

@@ -18,6 +18,7 @@
 import sys
 import rclpy  # pylint: disable=import-error
 import py_trees_ros  # pylint: disable=import-error
+from py_trees_ros_interfaces.srv import OpenSnapshotStream
 from scenario_execution import ScenarioExecution
 from .logging_ros import RosLogger
 from .marker_handler import MarkerHandler
@@ -67,7 +68,8 @@ def __init__(self) -> None:
             self.dry_run = self.node.get_parameter('dry_run').value
         if self.node.get_parameter('dot').value:
             self.render_dot = self.node.get_parameter('dot').value
-        super().__init__(debug=debug, log_model=log_model, live_tree=live_tree, scenario_file=scenario, output_dir=output_dir, dry_run=self.dry_run, render_dot=self.render_dot)
+        super().__init__(debug=debug, log_model=log_model, live_tree=live_tree, scenario_file=scenario,
+                         output_dir=output_dir, dry_run=self.dry_run, render_dot=self.render_dot)
 
     def _get_logger(self, debug):
         """
@@ -89,6 +91,14 @@ def setup_behaviour_tree(self, tree):
         """
         return py_trees_ros.trees.BehaviourTree(tree)
 
+    def post_setup(self):
+        request = OpenSnapshotStream.Request()
+        request.topic_name = "/scenario_execution/snapshots"
+        request.parameters.snapshot_period = sys.float_info.max
+        request.parameters.blackboard_data = True
+        response = OpenSnapshotStream.Response()
+        self.behaviour_tree._open_snapshot_stream(request, response)  # pylint: disable=protected-access
+
     def run(self) -> bool:
 
         executor = rclpy.executors.MultiThreadedExecutor()

scenario_execution_rviz/src/scenario_view.cpp

@@ -51,25 +51,15 @@ ScenarioView::ScenarioView(QWidget *parent) : rviz_common::Panel(parent) {
           SLOT(handleItemCollapsed(QTreeWidgetItem *)));
   connect(mScenarioView, SIGNAL(itemExpanded(QTreeWidgetItem *)), this,
           SLOT(handleItemExpanded(QTreeWidgetItem *)));
-
-  timer = new QTimer(this);
-  timer->setInterval(1000);
-  connect(timer, SIGNAL(timeout()), this, SLOT(requestBtPublishing()));
-  timer->start();
-  qDebug() << "initialisation done";
 }
 
 void ScenarioView::onInitialize() {
   _node = getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node();
 
   mBehaviorTreeSubscriber =
       _node->create_subscription<py_trees_ros_interfaces::msg::BehaviourTree>(
-          "/bt_snapshot_rviz", 10,
+          "/scenario_execution/snapshots", 10,
           std::bind(&ScenarioView::behaviorTreeChanged, this, _1));
-
-  mOpenSnapshotStreamClient =
-      _node->create_client<py_trees_ros_interfaces::srv::OpenSnapshotStream>(
-          "/scenario_execution/snapshot_streams/open");
 }
 
 void ScenarioView::handleItemCollapsed(QTreeWidgetItem *collapsedItem) {
@@ -93,39 +83,109 @@ int searchBehavior(const QString *child_id,
   return childPosition;
 }
 
+bool ScenarioView::isNewTree(
+    const py_trees_ros_interfaces::msg::BehaviourTree::SharedPtr previous,
+    const py_trees_ros_interfaces::msg::BehaviourTree::SharedPtr current)
+    const {
+
+  bool isNew = false;
+  if (previous != nullptr) {
+    isNew = false;
+    if (previous->behaviours.size() != current->behaviours.size()) {
+      isNew = true;
+    }
+    for (size_t i = 0; (i < current->behaviours.size()) && !isNew; i++) {
+      if ((current->behaviours.at(i).own_id.uuid !=
+           previous->behaviours.at(i).own_id.uuid) ||
+          (current->behaviours.at(i).name != previous->behaviours.at(i).name)) {
+        isNew = true;
+      }
+    }
+  } else {
+    isNew = true;
+  }
+  return isNew;
+}
+
 void ScenarioView::behaviorTreeChanged(
     const py_trees_ros_interfaces::msg::BehaviourTree::SharedPtr msg) {
-  if (treeWidgetBuilt == false || msg->changed == true) {
+  bool isNew = isNewTree(mPreviousMsg, msg);
+  mPreviousMsg = msg;
+
+  if (isNew) {
     QList<QTreeWidgetItem *> items;
     mScenarioView->clear();
     populateTree(items, msg);
+    if (items.size() > 0) {
+      static auto bg = items[0]->background(0);
+      items[0]->setBackground(0, bg);
+      items[0]->setBackground(1, bg);
+    }
     mScenarioView->insertTopLevelItems(0, items);
 
-    // expand everything if its run forthe first time
-    // else only expand already expanded item
-    if (treeWidgetBuilt) {
-      for (auto const &item : items) {
-        if (collapsedStates->value(item->text(2))) {
-          mScenarioView->expandItem(item);
+    // expand everything
+    for (auto const &item : items) {
+      mScenarioView->expandItem(item);
+    }
+  } else {
+    QTreeWidgetItemIterator it(mScenarioView);
+    while (*it) {
+      auto msg_behavior = msg->behaviours.begin();
+      while (msg_behavior < msg->behaviours.end()) {
+        if ((*it)->data(2, 0) ==
+            ConvertedBehavior::uuidToQString(msg_behavior->own_id.uuid)) {
+          break;
         }
+        msg_behavior++;
       }
-    } else {
-      for (auto const &item : items) {
-        mScenarioView->expandItem(item);
+
+      if (msg_behavior == msg->behaviours.end()) {
+        qDebug() << "Cannot find corresponding behavior";
+        break;
       }
+
+      ConvertedBehavior elem(*msg_behavior);
+      if ((*it)->data(2, 0) == elem.own_id) {
+        setIcon(elem.status, *it);
+
+        (*it)->setData(1, 0, elem.message);
+      }
+      ++it;
     }
+  }
 
-    // save the state/new state of the collapsed items in the QMap
-    int qMapcounter = 0;
-    collapsedStates->clear();
-    for (auto const &item : items) {
-      collapsedStates->insert(item->text(2), item->isExpanded());
-      qMapcounter++;
+  // set scenario result, if received
+  if (mScenarioView->topLevelItem(0)) {
+
+    for (auto it = msg->blackboard_on_visited_path.begin();
+         it != msg->blackboard_on_visited_path.end(); it++) {
+      QString prefix = QString("/");
+      prefix += mScenarioView->topLevelItem(0)->data(0, 0).toString();
+      prefix += "/";
+      if ((prefix + "end" == QString::fromStdString(it->key)) &&
+          ("True" == QString::fromStdString(it->value))) {
+        mScenarioView->topLevelItem(0)->setBackground(0, Qt::green);
+        mScenarioView->topLevelItem(0)->setBackground(1, Qt::green);
+      }
+      if ((prefix + "fail" == QString::fromStdString(it->key)) &&
+          ("True" == QString::fromStdString(it->value))) {
+        mScenarioView->topLevelItem(0)->setBackground(0, Qt::red);
+        mScenarioView->topLevelItem(0)->setBackground(1, Qt::red);
+      }
     }
+  }
+}
 
-    treeWidgetBuilt = true;
+void ScenarioView::setIcon(int status, QTreeWidgetItem *item) const {
+  if (status == 1) {
+    item->setIcon(0, waitingIcon);
+  } else if (status == 2) {
+    item->setIcon(0, runningIcon);
+  } else if (status == 3) {
+    item->setIcon(0, successIcon);
+  } else if (status == 4) {
+    item->setIcon(0, failedIcon);
   }
-  timer->start();
 }
 
 void ScenarioView::populateTree(
@@ -146,15 +206,7 @@ void ScenarioView::populateTree(
 
     items.append(new QTreeWidgetItem(nodeName));
 
-    if (behavior->status == 1) {
-      items.last()->setIcon(0, waitingIcon);
-    } else if (behavior->status == 2) {
-      items.last()->setIcon(0, runningIcon);
-    } else if (behavior->status == 3) {
-      items.last()->setIcon(0, successIcon);
-    } else if (behavior->status == 4) {
-      items.last()->setIcon(0, failedIcon);
-    }
+    setIcon(behavior->status, items.last());
   }
 
   int parentPosition = 0;
@@ -171,21 +223,6 @@ void ScenarioView::populateTree(
   }
 }
 
-void ScenarioView::requestBtPublishing() {
-  if (!mOpenSnapshotStreamClient->wait_for_service(
-          std::chrono::milliseconds(100))) {
-    // RCLCPP_WARN(
-    //     rclcpp::get_logger("rclcpp"),
-    //     "Failed to call service OpenSnapshotStream. Will try to reconnect");
-    treeWidgetBuilt = false;
-    timer->start();
-  }
-  auto request = std::make_shared<
-      py_trees_ros_interfaces::srv::OpenSnapshotStream::Request>();
-  request->topic_name = "/bt_snapshot_rviz";
-  auto result = mOpenSnapshotStreamClient->async_send_request(request);
-}
-
 } // end namespace scenario_execution_rviz
 
 #include <pluginlib/class_list_macros.hpp>

scenario_execution_rviz/src/scenario_view.h

@@ -45,7 +45,6 @@
 #include <cstdio>
 #include <iomanip>
 #include <py_trees_ros_interfaces/msg/behaviour_tree.hpp>
-#include <py_trees_ros_interfaces/srv/open_snapshot_stream.hpp>
 #include <rviz_common/panel.hpp>
 #include <rviz_common/view_manager.hpp>
 #include <tf2/utils.h>
@@ -66,7 +65,6 @@ class ScenarioView : public rviz_common::Panel {
   ScenarioView(QWidget *parent = 0);
 
 protected Q_SLOTS:
-  void requestBtPublishing();
   void handleItemCollapsed(QTreeWidgetItem *collapsedItem);
   void handleItemExpanded(QTreeWidgetItem *expandedItem);
 
@@ -78,17 +76,22 @@ protected Q_SLOTS:
   void populateTree(
       QList<QTreeWidgetItem *> &items,
       const py_trees_ros_interfaces::msg::BehaviourTree::SharedPtr msg);
+
+  bool isNewTree(
+      const py_trees_ros_interfaces::msg::BehaviourTree::SharedPtr previous,
+      const py_trees_ros_interfaces::msg::BehaviourTree::SharedPtr current)
+      const;
+  void setIcon(int status, QTreeWidgetItem *item) const;
+
   rclcpp::Subscription<py_trees_ros_interfaces::msg::BehaviourTree>::SharedPtr
       mBehaviorTreeSubscriber;
-  rclcpp::Client<py_trees_ros_interfaces::srv::OpenSnapshotStream>::SharedPtr
-      mOpenSnapshotStreamClient;
 
   rclcpp::Node::SharedPtr _node;
+  py_trees_ros_interfaces::msg::BehaviourTree::SharedPtr mPreviousMsg;
 
   QTreeWidget *mScenarioView;
   TreeModel *mTreeModel;
   bool treeWidgetBuilt = false;
-  QTimer *timer;
   QMap<QString, bool> *collapsedStates;
 
   // icons
@@ -133,7 +136,8 @@ class ConvertedBehavior : public QObject {
 
   QList<ConvertedBehavior *> mBehaviorList;
 
-  QString uuidToQString(const std::array<unsigned char, 16> &uuid) {
+  static const QString
+  uuidToQString(const std::array<unsigned char, 16> &uuid) {
     QString result;
 
     for (const auto &element : uuid) {

simulation/gazebo/tb4_sim_scenario/launch/ignition_launch.py

@@ -25,6 +25,7 @@
 from launch.actions import DeclareLaunchArgument, ExecuteProcess, Shutdown
 from launch.actions import SetEnvironmentVariable
 from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.conditions import UnlessCondition
 from launch_ros.actions import Node
 
 
@@ -46,6 +47,8 @@ def generate_launch_description():
     pkg_irobot_create_ignition_plugins = get_package_share_directory(
         'irobot_create_ignition_plugins')
 
+    headless = LaunchConfiguration('headless')
+
     arguments = [
         DeclareLaunchArgument('use_sim_time', default_value='true',
                               choices=['true', 'false'],
@@ -93,6 +96,7 @@ def generate_launch_description():
     )
 
     ignition_gazebo_gui = ExecuteProcess(
+        condition=UnlessCondition(headless),
         cmd=['ruby', '/usr/bin/ign', 'gazebo', '-g', '-v', '4', '--gui-config',
              PathJoinSubstitution([pkg_turtlebot4_ignition_bringup, 'gui', 'gui.config']), '--force-version', '6'],
         output='screen',