Feature idle trail (BehaviorTree#72)

* add idle trails

* optimize replay log row selection with keeping transition order

Co-authored-by: Мозговой Олег Владимирович <mozgovoy@nkbvs.ttn.ru>

bt_editor/mainwindow.cpp

@@ -1461,22 +1461,54 @@ bool MainWindow::SavedState::operator ==(const MainWindow::SavedState &other) co
     return true;
 }
 
+void MainWindow::resetTreeStyle(AbsBehaviorTree &tree){
+    //printf("resetTreeStyle\n");
+    QtNodes::NodeStyle  node_style;
+    QtNodes::ConnectionStyle conn_style;
+
+    for(auto abs_node: tree.nodes()){
+        auto gui_node = abs_node.graphic_node;
+
+        gui_node->nodeDataModel()->setNodeStyle( node_style );
+        gui_node->nodeGraphicsObject().update();
+
+        const auto& conn_in = gui_node->nodeState().connections(PortType::In, 0 );
+        if(conn_in.size() == 1)
+        {
+            auto conn = conn_in.begin()->second;
+            conn->setStyle( conn_style );
+            conn->connectionGraphicsObject().update();
+        }
+    }
+}
+
 void MainWindow::onChangeNodesStatus(const QString& bt_name,
                                      const std::vector<std::pair<int, NodeStatus> > &node_status)
 {
     auto tree = BuildTreeFromScene( getTabByName(bt_name)->scene() );
 
+    std::vector<NodeStatus> vec_last_status(tree.nodesCount());
+
+    // printf("---\n");
+
     for (auto& it: node_status)
     {
         const int index = it.first;
         const NodeStatus status = it.second;
         auto& abs_node = tree.nodes().at(index);
 
+        // printf("%3d: %d, %s\n", index, (int)it.second, abs_node.instance_name.toStdString().c_str());
+
+        if(index == 1 && it.second == NodeStatus::RUNNING)
+            resetTreeStyle(tree);
+
         auto gui_node = abs_node.graphic_node;
-        auto style = getStyleFromStatus( status );
+        auto style = getStyleFromStatus( status, vec_last_status[index] );
         gui_node->nodeDataModel()->setNodeStyle( style.first );
         gui_node->nodeGraphicsObject().update();
 
+        vec_last_status[index] = status;
+
         const auto& conn_in = gui_node->nodeState().connections(PortType::In, 0 );
         if(conn_in.size() == 1)
         {

bt_editor/mainwindow.h

@@ -57,6 +57,8 @@ class MainWindow : public QMainWindow
 
     const NodeModels &registeredModels() const;
 
+    void resetTreeStyle(AbsBehaviorTree &tree);
+
 public slots:
 
     void onAutoArrange();

bt_editor/sidepanel_replay.cpp

@@ -214,6 +214,10 @@ void SidepanelReplay::loadLog(const QByteArray &content)
     _transitions.clear();
     _transitions.reserve( (content.size() - 4 - bt_header_size) / 12 );
 
+    int idle_counter = _loaded_tree.nodes().size();
+    const int total_nodes = _loaded_tree.nodes().size();
+    int nearest_restart_transition_index = 0;
+
     for (size_t offset = 4+bt_header_size; offset < content.size(); offset += 12)
     {
         Transition transition;
@@ -225,6 +229,21 @@ void SidepanelReplay::loadLog(const QByteArray &content)
         transition.index = uid_to_index.at(uid);
         transition.prev_status = convert(flatbuffers::ReadScalar<Serialization::NodeStatus>(&buffer[offset+10] ));
         transition.status      = convert(flatbuffers::ReadScalar<Serialization::NodeStatus>(&buffer[offset+11] ));
+        transition.is_tree_restart = false;
+
+        if(transition.index == 1 &&
+                (transition.status == NodeStatus::RUNNING || transition.status == NodeStatus::IDLE) &&
+                idle_counter >= total_nodes - 1){
+            transition.is_tree_restart = true;
+            nearest_restart_transition_index = _transitions.size();
+        }
+
+        if(transition.prev_status != NodeStatus::IDLE && transition.status == NodeStatus::IDLE)
+            idle_counter++;
+        else if(transition.prev_status == NodeStatus::IDLE && transition.status != NodeStatus::IDLE)
+            idle_counter--;
+
+        transition.nearest_restart_transition_index = nearest_restart_transition_index;
 
         _transitions.push_back(transition);
     }
@@ -307,12 +326,10 @@ void SidepanelReplay::onRowChanged(int current_row)
         node_status.push_back( { index, NodeStatus::IDLE} );
     }
 
-    // THIS CAN BE OPTIMIZED, but it is so fast that I don't even care... for the time being.
-    for (int t = 0; t <= current_row; t++)
+    for (int t = _transitions[current_row].nearest_restart_transition_index; t <= current_row; t++)
     {
         auto& trans = _transitions[t];
-        node_status[ trans.index ].second = trans.status;
-        //qDebug() << trans.index << " : " << tr(toStr(trans.status));
+        node_status.push_back( { trans.index, trans.status} );
     }
 
     emit changeNodeStyle( bt_name, node_status );

bt_editor/sidepanel_replay.h

@@ -69,6 +69,8 @@ private slots:
         double timestamp;
         NodeStatus prev_status;
         NodeStatus status;
+        bool is_tree_restart;
+        int nearest_restart_transition_index;
     };
     std::vector<Transition> _transitions;
     std::vector< std::pair<double,int>> _timepoint;

bt_editor/utils.cpp

@@ -456,25 +456,54 @@ BuildTreeFromFlatbuffers(const Serialization::BehaviorTree *fb_behavior_tree)
 }
 
 std::pair<QtNodes::NodeStyle, QtNodes::ConnectionStyle>
-getStyleFromStatus(NodeStatus status)
+getStyleFromStatus(NodeStatus status, NodeStatus prev_status)
 {
     QtNodes::NodeStyle  node_style;
     QtNodes::ConnectionStyle conn_style;
 
+    float penWidth = 3.0;
+
     conn_style.HoveredColor = Qt::transparent;
 
+    //printf("status=%d, old=%d\n", status, prev_status);
+
     if( status == NodeStatus::IDLE )
     {
+        if(prev_status != NodeStatus::IDLE){
+            node_style.PenWidth *= penWidth;
+            node_style.HoveredPenWidth = node_style.PenWidth;
+
+            if( prev_status == NodeStatus::SUCCESS )
+            {
+                node_style.NormalBoundaryColor =
+                        node_style.ShadowColor = QColor(100, 150, 100);
+                conn_style.NormalColor = node_style.NormalBoundaryColor;
+            }
+            else if( prev_status == NodeStatus::RUNNING )
+            {
+                node_style.NormalBoundaryColor =
+                        node_style.ShadowColor =  QColor(150, 130, 40);
+                conn_style.NormalColor = node_style.NormalBoundaryColor;
+            }
+            else if( prev_status == NodeStatus::FAILURE )
+            {
+                node_style.NormalBoundaryColor =
+                        node_style.ShadowColor = QColor(150, 80, 80);
+                conn_style.NormalColor = node_style.NormalBoundaryColor;
+            }
+        }
+
         return {node_style, conn_style};
     }
 
-    node_style.PenWidth *= 3.0;
+    node_style.PenWidth *= penWidth;
     node_style.HoveredPenWidth = node_style.PenWidth;
 
     if( status == NodeStatus::SUCCESS )
     {
         node_style.NormalBoundaryColor =
                 node_style.ShadowColor = QColor(51, 200, 51);
+                node_style.ShadowColor = QColor(51, 250, 51);
         conn_style.NormalColor = node_style.NormalBoundaryColor;
     }
     else if( status == NodeStatus::RUNNING )

bt_editor/utils.h

@@ -27,7 +27,7 @@ AbsBehaviorTree BuildTreeFromXML(const QDomElement &bt_root, const NodeModels &m
 void NodeReorder(QtNodes::FlowScene &scene, AbsBehaviorTree &abstract_tree );
 
 std::pair<QtNodes::NodeStyle, QtNodes::ConnectionStyle>
- getStyleFromStatus(NodeStatus status);
+getStyleFromStatus(NodeStatus status, NodeStatus prev_status);
 
 QtNodes::Node* GetParentNode(QtNodes::Node* node);