robossembler/runtime
7
0
Fork 0
Code Issues 5 Pull requests Projects Releases Packages Wiki Activity Actions

add mode "benchmark"

Browse source
This commit is contained in:
shalenikol 2024-12-12 20:10:29 +03:00
parent 6ca5bbe646
commit ba4e51d06f
7 changed files with 222 additions and 9 deletions
Download patch file Download diff file Expand all files Collapse all files

30
rbs_bt_executor/scripts/rbs_interface.py
View file

@ -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):