diff --git a/rbs_bt_executor/launch/interface.launch.py b/rbs_bt_executor/launch/interface.launch.py index 9527d4e..ba2de13 100644 --- a/rbs_bt_executor/launch/interface.launch.py +++ b/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)]) diff --git a/rbs_bt_executor/scripts/rbs_interface.py b/rbs_bt_executor/scripts/rbs_interface.py index 1678efc..4836604 100755 --- a/rbs_bt_executor/scripts/rbs_interface.py +++ b/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): diff --git a/test_examples/README.md b/test_examples/README.md index 2ff21e7..2e6426d 100644 --- a/test_examples/README.md +++ b/test_examples/README.md @@ -11,22 +11,56 @@ source install/setup.bash ``` * 1 - запуск интерфейсной ноды с серверами навыков -```bash +```sh ros2 launch rbs_bt_executor interface.launch.py bt_path:=src/robossembler-ros2/test_examples/bt_default ``` * 2 - запуск runtime с роботом в сцене -```bash +```sh ros2 launch rbs_bringup rbs_bringup.launch.py ``` * 3 - запуск Gazebo (для визуализации процесса) -```bash +```sh ign gazebo -g ``` * 4 - запуск дерева поведения -```bash +```sh ros2 launch rbs_bt_executor rbs_bt_web.launch.py bt_path:=src/robossembler-ros2/test_examples/bt_default ``` Чтобы наблюдать сцену с движением робота, нужно перейти в окно Gazebo. ![gazebo_rbs_arm](gazebo_rbs_arm.png) *визуализация сцены с роботом* + +## Пример запуска симуляции в режиме `benchmark` для оценки управления роботом через разные контроллеры + +Запустить последовательно четыре терминала и выполнить запуск 3-х launch-файлов и симулятора Gazebo: +Во всех терминалах выполнить предварительную настройку: +```sh +# ROS2 +source /opt/ros/humble/setup.bash +# Robossembler +cd ~/robossembler-ws +source install/setup.bash +``` + +* 1 - запуск интерфейсной ноды в режиме `benchmark` +```sh +ros2 launch rbs_bt_executor interface.launch.py bt_path:=src/robossembler-ros2/test_examples/bt_benchmark mode:=benchmark +``` +* 2 - запуск runtime с роботом в сцене +```sh +ros2 launch rbs_bringup rbs_bringup.launch.py +``` +* 3 - запуск Gazebo (для визуализации процесса) +```sh +ign gazebo -g +``` +* 4 - запуск дерева поведения +```sh +ros2 launch rbs_bt_executor rbs_bt_web.launch.py bt_path:=src/robossembler-ros2/test_examples/bt_benchmark +``` + +В терминале 1 (интерфейсная нода) будет отражаться время выполнения каждого действия: + +![benchmark_result](benchmark_result.png) +*пример вывода журнала* diff --git a/test_examples/benchmark_result.png b/test_examples/benchmark_result.png new file mode 100644 index 0000000..14c54e0 Binary files /dev/null and b/test_examples/benchmark_result.png differ diff --git a/test_examples/bt_benchmark/bt.xml b/test_examples/bt_benchmark/bt.xml new file mode 100644 index 0000000..60e94fa --- /dev/null +++ b/test_examples/bt_benchmark/bt.xml @@ -0,0 +1,9 @@ + + + + + + + + + \ No newline at end of file diff --git a/test_examples/bt_benchmark/skills.json b/test_examples/bt_benchmark/skills.json new file mode 100644 index 0000000..7e371c7 --- /dev/null +++ b/test_examples/bt_benchmark/skills.json @@ -0,0 +1,135 @@ +{ + "skills": [ + { + "sid": "a", + "SkillPackage": { "name": "Robossembler", "version": "1.0", "format": "1" }, + "Module": { "node_name": "skill_mtp", "name": "MoveToPose", "description": "Move to the starting position" }, + "Launch": { "executable": "skill_movetopose.py", "package": "rbss_movetopose" }, + "BTAction": [ + { + "name": "move", + "type": "action", + "param": [ + { + "type": "move_to_pose", + "dependency": { "robot_name": "rbs_arm", + "pose": { "position": {"x":-0.45, "y":0.0, "z":0.3}, "orientation": {"x":0.0, "y":1.0, "z":0.0, "w": 0.0} } } + } + ], + "typeAction": "ACTION" + } + ], + "topicsOut": [], + "Settings": { + "output": { + "params": [ + { + "name": "server_name", + "value": "mtp_jtc", + "value2": "mtp_moveit", + "value3": "mtp_cart" + }, + { + "name": "duration", + "value": "5.0" + }, + { + "name": "end_effector_velocity", + "value": "1.0" + }, + { + "name": "end_effector_acceleration", + "value": "1.0" + } + ] + } + } + }, + { + "sid": "b", + "SkillPackage": { "name": "Robossembler", "version": "1.0", "format": "1" }, + "Module": { "node_name": "skill_mtp", "name": "MoveToPose", "description": "Move to pose with joint trajectory controller" }, + "Launch": { "executable": "skill_movetopose.py", "package": "rbss_movetopose" }, + "BTAction": [ + { + "name": "move", + "type": "action", + "param": [ + { + "type": "move_to_pose", + "dependency": { "robot_name": "rbs_arm", + "pose": { "position": {"x":-0.1, "y":-0.3, "z":0.3}, "orientation": {"x":0.0, "y":1.0, "z":0.0, "w": 0.0} } } + } + ], + "typeAction": "ACTION" + } + ], + "topicsOut": [], + "Settings": { + "output": { + "params": [ + { + "name": "server_name", + "value": "mtp_jtc" + }, + { + "name": "duration", + "value": "5.0" + }, + { + "name": "end_effector_velocity", + "value": "1.0" + }, + { + "name": "end_effector_acceleration", + "value": "1.0" + } + ] + } + } + }, + { + "sid": "c", + "SkillPackage": { "name": "Robossembler", "version": "1.0", "format": "1" }, + "Module": { "node_name": "skill_mtp", "name": "MoveToPose", "description": "Move to pose with cartesian controller" }, + "Launch": { "executable": "skill_movetopose.py", "package": "rbss_movetopose" }, + "BTAction": [ + { + "name": "move", + "type": "action", + "param": [ + { + "type": "move_to_pose", + "dependency": { "robot_name": "rbs_arm", + "pose": { "position": {"x":-0.45, "y":0.0, "z":0.3}, "orientation": {"x":0.0, "y":1.0, "z":0.0, "w": 0.0} } } + } + ], + "typeAction": "ACTION" + } + ], + "topicsOut": [], + "Settings": { + "output": { + "params": [ + { + "name": "server_name", + "value": "mtp_cart" + }, + { + "name": "duration", + "value": "5.0" + }, + { + "name": "end_effector_velocity", + "value": "1.0" + }, + { + "name": "end_effector_acceleration", + "value": "1.0" + } + ] + } + } + } + ] +} \ No newline at end of file diff --git a/test_examples/bt_default/skills.json b/test_examples/bt_default/skills.json index d85cf6b..be33752 100644 --- a/test_examples/bt_default/skills.json +++ b/test_examples/bt_default/skills.json @@ -13,7 +13,7 @@ { "type": "move_to_pose", "dependency": { "robot_name": "rbs_arm", - "pose": { "position": {"x": -0.3, "y": -0.2, "z": 0.2}, "orientation": {"x":0.0, "y":1.0, "z":0.0, "w": 0.0} } } + "pose": { "position": {"x":-0.15, "y":-0.3, "z":0.3}, "orientation": {"x":0.0, "y":1.0, "z":0.0, "w": 0.0} } } } ], "typeAction": "ACTION"