small fixes and added test client

Author: sasjonge

scripts/knowrob_designator_client.py

@@ -1,22 +1,32 @@
 #!/usr/bin/env python3
 # -*- coding: utf-8 -*-
-# Test client for DesignatorResolveStart action
+# Full client for all KnowRob designator actions
 
 import rospy
 import actionlib
-from knowrob_designator.msg import DesignatorResolutionStartAction, DesignatorResolutionStartGoal
-from time import time
+import uuid
+from time import sleep
+from knowrob_designator.msg import (
+    DesignatorInitAction, DesignatorInitGoal,
+    DesignatorResolutionStartAction, DesignatorResolutionStartGoal,
+    DesignatorResolutionFinishedAction, DesignatorResolutionFinishedGoal,
+    DesignatorExecutionStartAction, DesignatorExecutionStartGoal,
+    DesignatorExecutionFinishedAction, DesignatorExecutionFinishedGoal
+)
 
-def main():
-    rospy.init_node('designator_resolve_test_client')
-
-    client = actionlib.SimpleActionClient('/knowrob/designator/resolving_started', DesignatorResolutionStartAction)
-    rospy.loginfo("Waiting for action server...")
+def send_action(client, goal, label):
+    rospy.loginfo(f"[{label}] Waiting for action server...")
     client.wait_for_server()
+    rospy.loginfo(f"[{label}] Sending goal...")
+    client.send_goal(goal)
+    client.wait_for_result()
+    result = client.get_result()
+    rospy.loginfo(f"[{label}] Result: success={result.success}, message='{result.message}''")
+
+def main():
+    rospy.init_node('knowrob_designator_full_test_client')
 
-    goal = DesignatorResolutionStartGoal()
-    goal.designator_id = "desig_001"
-    goal.json_designator = """
+    json_designator = """
     {
       "anAction": {
         "type": "Transporting",
@@ -37,14 +47,75 @@ def main():
       }
     }
     """
-    goal.stamp = rospy.Time.now()
 
-    rospy.loginfo("Sending goal...")
-    client.send_goal(goal)
-    client.wait_for_result()
+    designator_id = f"desig_{uuid.uuid4()}"
+    resolved_id = f"desig_{uuid.uuid4()}"
+    now = rospy.Time.now()
 
-    result = client.get_result()
-    rospy.loginfo(f"Result: success={result.success}, message='{result.message}'")
+    # 1. DesignatorInit
+    init_client = actionlib.SimpleActionClient('/knowrob/designator/init', DesignatorInitAction)
+    init_goal = DesignatorInitGoal()
+    init_goal.designator_id = designator_id
+    init_goal.parent_id = ""  # root designator
+    init_goal.json_designator = json_designator
+    init_goal.stamp = now
+    send_action(init_client, init_goal, "Init")
+
+    # 2. DesignatorResolvingStarted
+    resolving_client = actionlib.SimpleActionClient('/knowrob/designator/resolving_started', DesignatorResolutionStartAction)
+    resolving_goal = DesignatorResolutionStartGoal()
+    resolving_goal.designator_id = designator_id
+    resolving_goal.json_designator = json_designator
+    resolving_goal.stamp = now
+    send_action(resolving_client, resolving_goal, "ResolveStart")
+
+    # 3. DesignatorResolutionFinished with resolved target
+    resolved_client = actionlib.SimpleActionClient('/knowrob/designator/resolving_finished', DesignatorResolutionFinishedAction)
+
+    resolved_designator = """
+    {
+      "anAction": {
+        "type": "Transporting",
+        "objectActedOn": {
+          "anObject": {
+            "type": "Milk"
+          }
+        },
+        "target": {
+          "pose": {
+            "x": 1.2,
+            "y": 0.8,
+            "z": 0.75,
+            "frame": "map"
+          }
+        }
+      }
+    }
+    """
+
+    resolved_goal = DesignatorResolutionFinishedGoal()
+    resolved_goal.designator_id = resolved_id
+    resolved_goal.resolved_from_id = designator_id
+    resolved_goal.json_designator = resolved_designator
+    resolved_goal.stamp = rospy.Time.now()
+    send_action(resolved_client, resolved_goal, "ResolveFinished")
+
+
+    # 4. DesignatorExecutionStart
+    exec_start_client = actionlib.SimpleActionClient('/knowrob/designator/execution_start', DesignatorExecutionStartAction)
+    exec_start_goal = DesignatorExecutionStartGoal()
+    exec_start_goal.designator_id = resolved_id
+    exec_start_goal.json_designator = resolved_designator
+    exec_start_goal.stamp = now
+    send_action(exec_start_client, exec_start_goal, "ExecutionStart")
+
+    # 5. DesignatorExecutionFinished
+    exec_finished_client = actionlib.SimpleActionClient('/knowrob/designator/execution_finished', DesignatorExecutionFinishedAction)
+    exec_finished_goal = DesignatorExecutionFinishedGoal()
+    exec_finished_goal.designator_id = resolved_id
+    exec_finished_goal.json_designator = resolved_designator
+    exec_finished_goal.stamp = now
+    send_action(exec_finished_client, exec_finished_goal, "ExecutionFinished")
 
 if __name__ == '__main__':
     main()

scripts/knowrob_designator_service.py

@@ -35,7 +35,7 @@ def __init__(self):
             auto_start=False
         )
         self.resolve_finished_server = actionlib.SimpleActionServer(
-            'knowrob/designator_resolving_finished',
+            'knowrob/designator/resolving_finished',
             DesignatorResolutionFinishedAction,
             execute_cb=self.handle_resolve_finished,
             auto_start=False
@@ -72,7 +72,7 @@ def __init__(self):
     def handle_init(self, goal):
         rospy.loginfo(f"Init Designator: {goal.designator_id}")
         rospy.logdebug(f"Full JSON:\n{goal.json_designator}")
-        result = DesignatorInitResult(success=True, message="Designator init logged.", status="done")
+        result = DesignatorInitResult(success=True, message="Designator init logged.")
         self.init_server.set_succeeded(result)
 
     def handle_resolve_start(self, goal):
@@ -117,17 +117,17 @@ def handle_resolve_start(self, goal):
 
     def handle_resolve_finished(self, goal):
         rospy.loginfo(f"Finished Resolving Designator: {goal.designator_id} from {goal.resolved_from_id}")
-        result = DesignatorResolutionFinishedResult(success=True, message="Resolution finished logged.", status="done")
+        result = DesignatorResolutionFinishedResult(success=True, message="Resolution finished logged.")
         self.resolve_finished_server.set_succeeded(result)
 
     def handle_exec_start(self, goal):
         rospy.loginfo(f"Execution Started: {goal.designator_id}")
-        result = DesignatorExecutionStartResult(success=True, message="Execution started logged.", status="done")
+        result = DesignatorExecutionStartResult(success=True, message="Execution started logged.")
         self.exec_start_server.set_succeeded(result)
 
     def handle_exec_finished(self, goal):
         rospy.loginfo(f"Execution Finished: {goal.designator_id}")
-        result = DesignatorExecutionFinishedResult(success=True, message="Execution finished logged.", status="done")
+        result = DesignatorExecutionFinishedResult(success=True, message="Execution finished logged.")
         self.exec_finished_server.set_succeeded(result)
 
 if __name__ == '__main__':

src/knowrob_designator/designator_parser.py

@@ -1,4 +1,10 @@
+import uuid
+
 class DesignatorParser:
+    def __init__(self):
+        self.triples = []
+
+    # Mockup for the parse method
     def parse(self, designator):
         """
         Parse a designator dictionary into RDF-style triples.
@@ -23,3 +29,56 @@ def parse(self, designator):
             ("e", "name", "Table1")
         ]
         return triples
+
+    def parse(self, designator_json, root=None):
+        """
+        Iteratively convert a nested designator JSON into RDF-style triples.
+
+        :param designator_json: dict (parsed JSON structure)
+        :param root: optional root URI
+        :return: list of (subject, predicate, object) triples
+        """
+        self.triples.clear()
+        root_id = root or self._new_designator_id()
+
+        # Stack of (current_subject, current_object)
+        stack = [(root_id, designator_json)]
+
+        while stack:
+            subject, current = stack.pop()
+
+            if not isinstance(current, dict):
+                continue
+
+            for key, value in current.items():
+                predicate = self._predicate(key)
+
+                if isinstance(value, dict):
+                    sub_id = self._new_designator_id()
+                    self.triples.append((subject, predicate, sub_id))
+                    stack.append((sub_id, value))
+
+                elif isinstance(value, list):
+                    for v in value:
+                        if isinstance(v, dict):
+                            sub_id = self._new_designator_id()
+                            self.triples.append((subject, predicate, sub_id))
+                            stack.append((sub_id, v))
+                        else:
+                            self.triples.append((subject, predicate, self._literal(v)))
+
+                else:
+                    self.triples.append((subject, predicate, self._literal(value)))
+
+        return self.triples
+
+    def _new_designator_id(self):
+        return f"desig:{uuid.uuid4()}"
+
+    def _predicate(self, key):
+        return f"knowrob:{key}"
+
+    def _literal(self, value):
+        if isinstance(value, str):
+            return f"\"{value}\""
+        return str(value)