Merge pull request #192 from JdeRobot/adding-bt-status-viewer

Adding BT status visualization

backend/ros_template/ros_template/execute_docker.py

@@ -1,22 +1,40 @@
+import functools
 import rclpy
+import py_trees
 from rclpy.node import Node
 import tree_factory
 import os
+from tree_tools import ascii_bt_to_json
 
 
 class TreeExecutor(Node):
     def __init__(self):
-
         super().__init__("tree_executor_node")
-
         # Get the path to the root of the package
         ws_path = "/workspace/code"
         tree_path = os.path.join(ws_path, "self_contained_tree.xml")
 
         factory = tree_factory.TreeFactory()
         self.tree = factory.create_tree_from_file(tree_path)
+        snapshot_visitor = py_trees.visitors.SnapshotVisitor()
+        self.tree.add_post_tick_handler(
+            functools.partial(self.post_tick_handler, snapshot_visitor)
+        )
+        self.tree.visitors.append(snapshot_visitor)
         self.tree.tick_tock(period_ms=50)
 
+    def post_tick_handler(self, snapshot_visitor, behaviour_tree):
+        with open("/tmp/tree_state", "w") as f:
+            ascii_bt_to_json(
+                py_trees.display.ascii_tree(
+                    behaviour_tree.root,
+                    visited=snapshot_visitor.visited,
+                    previously_visited=snapshot_visitor.visited,
+                ),
+                py_trees.display.ascii_blackboard(),
+                f,
+            )
+
     def spin_tree(self):
 
         try:
@@ -28,11 +46,9 @@ def spin_tree(self):
 
 
 def main():
-
     # Init the components
     rclpy.init()
     executor = TreeExecutor()
-
     # Spin the tree
     executor.spin_tree()
 

backend/tree_api/json_translator.py

@@ -107,6 +107,35 @@ def build_xml(node_models, link_models, tree_structure, node_id, xml_parent, ord
             )
 
 
+def build_tree_structure(node_models, link_models, tree_structure, node_id, order):
+
+    node_name = node_models[node_id]["name"]
+    data_ports = get_data_ports(node_models, link_models, node_id)
+
+    # Add data_ports as attributes to current_element
+    attributes = {"name": node_name, "id": node_id, "childs": []}
+
+    # Apply recursion to all its children
+    if node_id in tree_structure:
+        tree_structure[node_id] = sorted(
+            tree_structure[node_id],
+            key=lambda item: node_models[item]["y"],
+            reverse=order,
+        )  # Fixed: issue #73
+        for child_id in tree_structure[node_id]:
+            attributes["childs"].append(
+                build_tree_structure(
+                    node_models,
+                    link_models,
+                    tree_structure,
+                    child_id,
+                    order,
+                )
+            )
+
+    return attributes
+
+
 def get_start_node_id(node_models, link_models):
 
     start_node_id = ""
@@ -157,3 +186,26 @@ def translate(content, tree_path, raw_order):
     f = open(tree_path, "w")
     f.write(xml_string)
     f.close()
+
+
+def translate_tree_structure(content):
+    # Parse the JSON data
+    parsed_json = content
+
+    # Extract nodes and links information
+    node_models = parsed_json["layers"][1]["models"]
+    link_models = parsed_json["layers"][0]["models"]
+
+    # Get the tree structure
+    tree_structure = get_tree_structure(link_models, node_models)
+    # Get the order of bt: True = Ascendent; False = Descendent
+    # order = raw_order == "bottom-to-top"
+
+    # Generate XML
+    start_node_id = get_start_node_id(node_models, link_models)
+    print(start_node_id)
+    root = build_tree_structure(
+        node_models, link_models, tree_structure, start_node_id, False
+    )
+
+    return root

backend/tree_api/urls.py

@@ -19,6 +19,7 @@
     path("delete_file/", views.delete_file, name="delete_file"),
     path("save_file/", views.save_file, name="save_file"),
     path("translate_json/", views.translate_json, name="translate_json"),
+    path("get_tree_structure/", views.get_tree_structure, name="get_tree_structure"),
     path(
         "get_universe_configuration/",
         views.get_universe_configuration,

backend/tree_api/views.py

@@ -173,6 +173,37 @@ def get_project_graph(request):
         )
 
 
+@api_view(["GET"])
+def get_tree_structure(request):
+
+    project_name = request.GET.get("project_name")
+
+    # Generate the paths
+    base_path = os.path.join(settings.BASE_DIR, "filesystem")
+    project_path = os.path.join(base_path, project_name)
+    graph_path = os.path.join(project_path, "code/graph.json")
+
+    # Check if the project exists
+    if os.path.exists(graph_path):
+        try:
+            with open(graph_path, "r") as f:
+                graph_data = json.load(f)
+
+                # Get the tree structure
+                tree_structure = json_translator.translate_tree_structure(graph_data)
+
+            return JsonResponse({"success": True, "tree_structure": tree_structure})
+        except Exception as e:
+            return JsonResponse(
+                {"success": False, "message": f"Error reading file: {str(e)}"},
+                status=500,
+            )
+    else:
+        return Response(
+            {"error": "The project does not have a graph definition"}, status=404
+        )
+
+
 @api_view(["GET"])
 def get_universes_list(request):
 
@@ -629,7 +660,6 @@ def generate_app(request):
     # Get the parameters
     app_name = request.data.get("app_name")
     tree_graph = request.data.get("tree_graph")
-    print(tree_graph)
     bt_order = request.data.get("bt_order")
 
     # Make folder path relative  to Django app

backend/tree_gardener/tree_gardener/tree_tools.py

@@ -1,3 +1,4 @@
+import re
 import py_trees
 
 
@@ -46,3 +47,86 @@ def set_port_content(port_value, value):
 
     # Set the value for the key in the blackboard
     setattr(blackboard, key, value)
+
+
+########### ASCII BT STATUS TO JSON ############################################
+def ascii_state_to_state(state_raw):
+    letter = [x for x in state_raw]
+    state = letter[1]
+
+    match state:
+        case "*":
+            return "RUNNING"
+        case "o":
+            return "SUCCESS"
+        case "x":
+            return "FAILURE"
+        case "-":
+            return "INVALID"
+        case _:
+            return "INVALID"
+
+
+def ascii_tree_to_json(tree):
+    indent_levels = 4  # 4 spaces = 1 level deep
+    do_append_coma = False
+    last_indent_level = -1
+    json_str = '"tree":{'
+
+    # Remove escape chars
+    ansi_escape = re.compile(r"\x1B(?:[@-Z\\-_]|\[[0-?]*[ -/]*[@-~])")
+    tree = ansi_escape.sub("", tree)
+
+    for line in iter(tree.splitlines()):
+        entry = line.strip().split(" ")
+        name = entry[1]
+        if len(entry) == 2:
+            state = "NONE"
+        else:
+            state = ascii_state_to_state(entry[2])
+
+        indent = int((len(line) - len(line.lstrip())) / indent_levels)
+        if not (indent > last_indent_level):
+            json_str += "}" * (last_indent_level - indent + 1)
+
+        last_indent_level = indent
+
+        if do_append_coma:
+            json_str += ","
+        else:
+            do_append_coma = True
+        json_str += '"' + name + '":{'
+        json_str += f'"state":"{state}"'
+
+    json_str += "}" * (last_indent_level + 1) + "}"
+    return json_str
+
+
+def ascii_blackboard_to_json(blackboard):
+    json_str = '"blackboard":{'
+    do_append_coma = False
+
+    # FIX: [entry, value] = line.strip()[1:].split(":")
+
+    for line in iter(blackboard.splitlines()):
+        if "Blackboard Data" in line:
+            continue
+        if len(line.strip()) == 0:
+            continue
+        if do_append_coma:
+            json_str += ","
+        else:
+            do_append_coma = True
+        # Remove whitespaces with strip and remove / from entry
+        [entry, value] = line.strip()[1:].split(":")
+        json_str += f'"{entry.strip()}":"{value.strip()}"'
+    json_str += "}"
+    return json_str
+
+
+def ascii_bt_to_json(tree, blackboard, file):
+    file.write("{")
+    # file.write(f"{ascii_tree_to_json(tree)},{ascii_blackboard_to_json(blackboard)}")
+    file.write(f"{ascii_tree_to_json(tree)}")
+    file.write("}")
+    file.close()

frontend/src/App.css

@@ -241,4 +241,9 @@ h3 {
   --bt-action-default-color: red;
   --bt-tag-blackboard-background: #5ba498;
   --bt-tag-normal-background: #a45b67;
+
+  --bt-status-running: #ee942e;
+  --bt-status-success: #29ac29;
+  --bt-status-failure: #b11111;
+  --bt-status-invalid: #494949;
 }

frontend/src/App.tsx

@@ -11,6 +11,7 @@ import VncViewer from "./components/vnc_viewer/VncViewer";
 import ErrorModal from "./components/error_popup/ErrorModal";
 import axios from "axios";
 import EditorContainer from "./components/diagram_editor/EditorContainer";
+import DiagramVisualizerContainer from "./components/bt_status_visualizer/DiagramVisualizerContainer";
 import CommsManager from "./api_helper/CommsManager";
 import { loadProjectConfig } from "./api_helper/TreeWrapper";
 
@@ -30,6 +31,12 @@ const App = () => {
   const [diagramEditorReady, setDiagramEditorReady] = useState<boolean>(false);
   const [appRunning, setAppRunning] = useState<boolean>(false);
 
+  // TODO: temporary
+  // const [showExecStatus, setShowExecStatus] = useState<boolean>(true);
+  // const onSetShowExecStatus = () => {
+  //   setShowExecStatus(!showExecStatus)
+  // }
+
   // const defaultDark = window.matchMedia('(prefers-color-scheme: dark)').matches;
   // const [theme, setTheme] = useLocalStorage('theme', defaultDark ? 'dark' : 'light');
   ////////////////////// SETTINGS //////////////////////
@@ -128,6 +135,7 @@ const App = () => {
         settingsProps={settings}
         gazeboEnabled={gazeboEnabled}
         setGazeboEnabled={setGazeboEnabled}
+        // onSetShowExecStatus={onSetShowExecStatus}
         manager={manager}
       />
 
@@ -169,11 +177,20 @@ const App = () => {
         >
           <div style={{ flex: 1 }}>
             {currentProjectname ? (
-              <EditorContainer
-                projectName={currentProjectname}
-                setProjectEdited={setProjectChanges}
-                setGlobalJson={setModelJson}
-              />
+              <>
+                {true ? (
+                  <EditorContainer
+                    projectName={currentProjectname}
+                    setProjectEdited={setProjectChanges}
+                    setGlobalJson={setModelJson}
+                  />
+                ) : (
+                  <DiagramVisualizerContainer
+                    projectName={currentProjectname}
+                    manager={manager}
+                  />
+                )}
+              </>
             ) : (
               <p>Loading...</p>
             )}