add mode "benchmark"

rbs_bt_executor/launch/interface.launch.py

@@ -3,6 +3,7 @@
   ROS 2 launch program for Robossembler
 
   @shalenikol release 0.1
+  @shalenikol release 0.2 mode
 """
 import os
 import json
@@ -47,12 +48,15 @@ def launch_setup(context, *args, **kwargs):
     # Initialize Arguments
     bt_path = LaunchConfiguration("bt_path")
     bt_path = bt_path.perform(context)
+    mode = LaunchConfiguration("mode")
+    mode = mode.perform(context)
 
     skills = get_skill_list_(bt_path)
 
     rbs_interface = Node(
         package="rbs_bt_executor",
         executable="rbs_interface.py",
+        parameters = [{"mode": mode},{"use_sim_time": True}]
         # parameters = [{"bt_path": bt_path}] # can be included as default path
     )
     nodes_to_start = [rbs_interface]
@@ -63,8 +67,15 @@ def generate_launch_description():
     declared_arguments.append(
         DeclareLaunchArgument(
             "bt_path",
-            default_value="''",
+            default_value="",
             description="path to Behavior tree instance"
         )
     )
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "mode",
+            default_value="",
+            description="Run mode of the interface node"
+        )
+    )
     return LaunchDescription(declared_arguments + [OpaqueFunction(function=launch_setup)])

rbs_bt_executor/scripts/rbs_interface.py

@@ -6,9 +6,11 @@
   @shalenikol release 0.1
   @shalenikol release 0.2 BT v.4
   @shalenikol release 0.3 synchronize
+  @shalenikol release 0.4 mode:="benchmark"
 """
 import os
 import json
+import time
 
 import rclpy
 from rclpy.node import Node
@@ -29,6 +31,8 @@ from rbs_skill_interfaces.action import RbsBt as RbsBtAction
 CONDITION_SRV_NAME = "/condition"
 COMPLETION_SRV_NAME = "/completion"
 BT_PARAM     = "bt_path"
+MODE_PARAM   = "mode"
+MODE_BENCHMARK = "benchmark"
 NODE_NAME    = "rbs_interface"
 SERVICE_NAME = "rbs_interface_s"
 SERVER_NAME  = "rbs_interface_a"
@@ -40,9 +44,13 @@ class rbsInterface(Node):
     def __init__(self, node_name):
         """Construct the node."""
         self.bt_path = ""    # path to the current BehaviorTree
+        # self._mode = ""      # run mode of the interface node
         self.cfg_data = None # config for current action
         super().__init__(node_name)
         self.declare_parameter(BT_PARAM, rclpy.Parameter.Type.STRING)
+        self.declare_parameter(MODE_PARAM, rclpy.Parameter.Type.STRING)
+        self._mode = self.get_parameter(MODE_PARAM).get_parameter_value().string_value
+
         self.cb_group = ReentrantCallbackGroup()
         # for Condition
         self._service = self.create_service(RbsBt, SERVICE_NAME, self.service_callback, callback_group=self.cb_group)
@@ -62,6 +70,9 @@ class rbsInterface(Node):
             if parameter.name == BT_PARAM:
                 self.bt_path = parameter.value
                 # self.get_logger().info(f'$ {parameter.name}={parameter.value}')
+            # if parameter.name == MODE_PARAM:
+            #     self._mode = parameter.value
+            #     self.get_logger().info(f'$ {parameter.name}={parameter.value}')
         return SetParametersResult(successful=True)
 
     # def get_remote_parameter(self, remote_node_name, param_name):
@@ -164,11 +175,8 @@ class rbsInterface(Node):
                         ret = res.success
 
         elif oper_type == "action":
-            # cnt = 0
             while not self.run_condition(COMPLETION_SRV_NAME, command_data["name"]):
                 pass
-                # cnt += 1
-                # self.get_logger().info(f"action + run_condition: {cnt}")
             ret = True
 
         return ret
@@ -195,8 +203,24 @@ class rbsInterface(Node):
         is_success = False
         for comm in self.cfg_data[KEY_BTPARAM]:
             if comm["name"] == command:
+                start_time_r = time.perf_counter()
+                start_time = self.get_clock().now()  # Запоминаем время начала
+
                 is_success = self.run_condition(CONDITION_SRV_NAME, command) if isCondition else self.run_action(comm)
 
+                if self._mode == MODE_BENCHMARK:
+                    end_time_r = time.perf_counter()
+                    end_time = self.get_clock().now()  # Запоминаем время окончания
+                    elapsed_time = (end_time - start_time).nanoseconds / 1e6  # time in ms
+                    elapsed_time_r = (end_time_r - start_time_r) * 1000 # compute time in ms
+                    if elapsed_time > 1000:
+                        str_res = f"{elapsed_time/1000} s"
+                        str_comp = f"{elapsed_time_r/1000} s"
+                    else:
+                        str_res = f"{elapsed_time} ms"
+                        str_comp = f"{elapsed_time_r} ms"
+                    self.get_logger().info(f"*{typeBTnode}* Real time " + str_res + " / simulation time " + str_comp)
+
         return is_success
 
     def action_callback(self, goal_h):