add mode "benchmark"
This commit is contained in:
parent
6ca5bbe646
commit
ba4e51d06f
7 changed files with 222 additions and 9 deletions
|
@ -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)])
|
||||
|
|
|
@ -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):
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue