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):
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
||||
|
||||
*визуализация сцены с роботом*
|
||||
|
||||
## Пример запуска симуляции в режиме `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 (интерфейсная нода) будет отражаться время выполнения каждого действия:
|
||||
|
||||
|
||||
*пример вывода журнала*
|
||||
|
|
|
|||
BIN
test_examples/benchmark_result.png
Normal file
BIN
test_examples/benchmark_result.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 157 KiB |
9
test_examples/bt_benchmark/bt.xml
Normal file
9
test_examples/bt_benchmark/bt.xml
Normal file
|
|
@ -0,0 +1,9 @@
|
|||
<root BTCPP_format="4">
|
||||
<BehaviorTree ID="Main">
|
||||
<Sequence>
|
||||
<Action ID="RbsAction" do="MoveToPose" command="move" sid="a"></Action>
|
||||
<Action ID="RbsAction" do="MoveToPose" command="move" sid="b"></Action>
|
||||
<Action ID="RbsAction" do="MoveToPose" command="move" sid="c"></Action>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
||||
135
test_examples/bt_benchmark/skills.json
Normal file
135
test_examples/bt_benchmark/skills.json
Normal file
|
|
@ -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"
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
|
|
@ -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"
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue