more logs for planning

merlin2_arch/merlin2_planning_layer/merlin2_executor/merlin2_executor/merlin2_executor_node.py

@@ -17,6 +17,7 @@
 """ Merlin2 Executor Node """
 
 
+import time
 import rclpy
 
 from kant_dao import ParameterLoader
@@ -89,6 +90,7 @@ def __execute_server(self, goal_handle):
 
         blackboard = Blackboard()
         blackboard["result"] = result
+        blackboard["init_time"] = time.time()
 
         outcome = self(blackboard)
 
@@ -110,11 +112,8 @@ def __execute_server(self, goal_handle):
 
 def main():
     rclpy.init()
-
     node = Merlin2ExecutorNode()
-
     node.join_spin()
-
     rclpy.shutdown()
 
 

merlin2_arch/merlin2_planning_layer/merlin2_executor/merlin2_executor/states/merlin2_generate_pddl_state.py

@@ -16,6 +16,7 @@
 
 """ Generate PDDL State """
 
+import time
 from simple_node import Node
 from yasmin.blackboard import Blackboard
 from yasmin_ros import ServiceState
@@ -39,10 +40,14 @@ def create_request_handler(self, blackboard: Blackboard) -> GeneratePddl.Request
     def response_handler(self, blackboard: Blackboard, response: GeneratePddl.Response) -> str:
         blackboard["domain"] = response.domain
         blackboard["problem"] = response.problem
+        blackboard["result"].generate_pddl = True
+
+        elapsed_time = time.time() - blackboard["init_time"]
 
         self._node.get_logger().info(response.domain)
         self._node.get_logger().info(response.problem)
 
-        blackboard["result"].generate_pddl = True
+        self._node.get_logger().info(
+            f"Time from receiving goals to planning: {elapsed_time} seconds")
 
         return SUCCEED

merlin2_arch/merlin2_planning_layer/merlin2_pddl_generator/merlin2_pddl_generator/merlin2_pddl_generator_node.py

@@ -73,11 +73,8 @@ def __generate_pddl_srv(self,
 
 def main():
     rclpy.init()
-
     node = Merlin2PddlGeneratorNode()
-
     node.join_spin()
-
     rclpy.shutdown()