added object designator logging

Author: Sascha Jongebloed

CMakeLists.txt

@@ -16,6 +16,7 @@ catkin_python_setup()
 add_action_files(
   DIRECTORY action
   FILES
+    PushObjectDesignator.action
     DesignatorInit.action
     DesignatorResolutionStart.action
     DesignatorResolutionFinished.action

action/ObjectDesignator action/PushObjectDesignator.actionaction/ObjectDesignator renamed to action/PushObjectDesignator.action

@@ -1,7 +1,6 @@
 # Add or modify and object
 string json_designator
-time start
-time end
+time stamp
 ---
 bool success
 string message

scripts/knowrob_designator_client.py

@@ -7,6 +7,7 @@
 import uuid
 from time import sleep
 from knowrob_designator.msg import (
+    PushObjectDesignatorAction, PushObjectDesignatorGoal,
     DesignatorInitAction, DesignatorInitGoal,
     DesignatorResolutionStartAction, DesignatorResolutionStartGoal,
     DesignatorResolutionFinishedAction, DesignatorResolutionFinishedGoal,
@@ -25,7 +26,33 @@ def send_action(client, goal, label):
 
 def main():
     rospy.init_node('knowrob_designator_full_test_client')
-
+    
+    ##########################################################
+    ############### Object Designators ########################
+        
+    # 0. PushObjectDesignator
+    push_client = actionlib.SimpleActionClient('/knowrob/designator/push_object_designator', PushObjectDesignatorAction)
+    push_goal = PushObjectDesignatorGoal()
+    push_goal.json_designator = """
+    {
+      "anObject": {
+        "type": "Milk"
+        "pose": {
+          "x": 1.0, 
+          "y": 0.5,
+          "z": 0.75,
+          "frame": "map"
+        }
+      }
+    }
+    """
+    push_goal.stamp = rospy.Time.now()
+    send_action(push_client, push_goal, "PushObjectDesignator")
+    
+    ##########################################################
+    ############### Action Designators ########################
+    
+    # Create a designator ID and JSON designator
     json_designator = """
     {
       "anAction": {
@@ -47,7 +74,6 @@ def main():
       }
     }
     """
-
     designator_id = f"desig_{uuid.uuid4()}"
     resolved_id = f"desig_{uuid.uuid4()}"
     now = rospy.Time.now()

scripts/knowrob_designator_service.py

@@ -7,6 +7,7 @@
 
 # Import all designator action types
 from knowrob_designator.msg import (
+    PushObjectDesignatorAction, PushObjectDesignatorResult, PushObjectDesignatorFeedback,
     DesignatorInitAction, DesignatorInitResult, DesignatorInitFeedback,
     DesignatorResolutionStartAction, DesignatorResolutionStartResult, DesignatorResolutionStartFeedback,
     DesignatorResolutionFinishedAction, DesignatorResolutionFinishedResult, DesignatorResolutionFinishedFeedback,
@@ -22,6 +23,12 @@ def __init__(self):
         rospy.init_node('designator_logger_server')
 
         # Start an action server for each action type
+        self.push_object_server = actionlib.SimpleActionServer(
+            '/knowrob/designator/push_object_designator',
+            PushObjectDesignatorAction,
+            execute_cb=self.handle_push_object_designator,
+            auto_start=False
+        )
         self.init_server = actionlib.SimpleActionServer(
             '/knowrob/designator/init',
             DesignatorInitAction,
@@ -61,13 +68,20 @@ def __init__(self):
         self.parser = DesignatorParser()     
 
         # Start all servers
+        self.push_object_server.start()
         self.init_server.start()
         self.resolve_start_server.start()
         self.resolve_finished_server.start()
         self.exec_start_server.start()
         self.exec_finished_server.start()
 
         rospy.loginfo("DesignatorLoggerServer: all servers started.")
+        
+    def handle_push_object_designator(self, goal):
+        rospy.loginfo(f"Push Object Designator")
+        rospy.logdebug(f"Full JSON:\n{goal.json_designator}")
+        result = PushObjectDesignatorResult(success=True, message="Designator pushed.")
+        self.push_object_server.set_succeeded(result)
 
     def handle_init(self, goal):
         rospy.loginfo(f"Init Designator: {goal.designator_id}")