add mode "benchmark"

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

View file

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

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