add rosbag example
This commit is contained in:
parent
ba4e51d06f
commit
78454a6abb
4 changed files with 151 additions and 15 deletions
|
@ -36,20 +36,26 @@
|
|||
"name": "rdConfigure",
|
||||
"type_action": "action",
|
||||
"type": "run",
|
||||
"param": [],
|
||||
"result":[]
|
||||
"param": []
|
||||
},
|
||||
{ "name": "rdStop", "type_action": "action", "type": "stop", "param": [], "result": [] }
|
||||
{ "name": "rdStop", "type_action": "action", "type": "stop", "param": [] }
|
||||
],
|
||||
"Settings": [
|
||||
{ "name": "output_path", "value": "rbs_testbag", "description": "!!! Путь, куда запишется rosbag. Если он уже есть - перетрётся!" }
|
||||
"Settings": {
|
||||
"output": {
|
||||
"params": [
|
||||
{
|
||||
"name": "output_path",
|
||||
"value": "rbs_testbag"
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
"sid": "c",
|
||||
"SkillPackage": { "name": "Robossembler", "version": "1.0", "format": "1" },
|
||||
"Module": { "name": "MoveToPose", "description": "Move to Pose skill with cartesian controllers", "node_name": "mtp_cartesian" },
|
||||
"Launch": { "package": "rbss_movetopose", "executable": "mtp_cartesian.py" },
|
||||
"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",
|
||||
|
@ -64,12 +70,30 @@
|
|||
]
|
||||
}
|
||||
],
|
||||
"Settings": [
|
||||
{ "name": "server_name", "value": "cartesian_move_to_pose" },
|
||||
{ "name": "end_effector_velocity", "value": 1.0 },
|
||||
{ "name": "end_effector_acceleration", "value": 1.0 }
|
||||
"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"
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
||||
```
|
||||
|
@ -77,11 +101,11 @@
|
|||
Запустить последовательно три терминала и выполнить запуск 3-х launch-файлов:
|
||||
* 1 - запуск интерфейсной ноды с серверами навыков
|
||||
```bash
|
||||
ros2 launch rbs_interface interface.launch.py bt_path:=/path/to/bt
|
||||
ros2 launch rbs_bt_executor interface.launch.py bt_path:=/path/to/bt
|
||||
```
|
||||
* 2 - запуск робота в сцене
|
||||
```bash
|
||||
ros2 launch rbs_bringup single_robot.launch.py
|
||||
ros2 launch rbs_bringup rbs_bringup.launch.py
|
||||
```
|
||||
* 3 - запуск дерева поведения
|
||||
```bash
|
||||
|
|
|
@ -64,3 +64,33 @@ ros2 launch rbs_bt_executor rbs_bt_web.launch.py bt_path:=src/robossembler-ros2/
|
|||
|
||||

|
||||
*пример вывода журнала*
|
||||
|
||||
## Пример запуска симуляции с записью rosbag-журнала для демонстрации в веб.
|
||||
|
||||
Запустить последовательно три терминала и выполнить запуск 3-х launch-файлов.
|
||||
Во всех терминалах выполнить предварительную настройку:
|
||||
```sh
|
||||
# ROS2
|
||||
source /opt/ros/humble/setup.bash
|
||||
# Robossembler
|
||||
cd ~/robossembler-ws
|
||||
source install/setup.bash
|
||||
```
|
||||
|
||||
* 1 - запуск интерфейсной ноды
|
||||
```sh
|
||||
ros2 launch rbs_bt_executor interface.launch.py bt_path:=src/robossembler-ros2/test_examples/bt_rosbag
|
||||
```
|
||||
* 2 - запуск runtime с роботом в сцене
|
||||
```sh
|
||||
ros2 launch rbs_bringup rbs_bringup.launch.py
|
||||
```
|
||||
* 3 - запуск дерева поведения
|
||||
```sh
|
||||
ros2 launch rbs_bt_executor rbs_bt_web.launch.py bt_path:=src/robossembler-ros2/test_examples/bt_rosbag
|
||||
```
|
||||
Для визуализации можно дополнительно запустить симулятор Gazebo (терминал 4):
|
||||
```sh
|
||||
ign gazebo -g
|
||||
```
|
||||
После выполнения дерева в папке ~/robossembler-ws/rbs_testbag сохранится журнал rosbag.
|
9
test_examples/bt_rosbag/bt.xml
Normal file
9
test_examples/bt_rosbag/bt.xml
Normal file
|
@ -0,0 +1,9 @@
|
|||
<root BTCPP_format="4">
|
||||
<BehaviorTree ID="Main">
|
||||
<Sequence>
|
||||
<Action ID="RbsAction" do="RecordingDemo" command="rdConfigure" sid="a"></Action>
|
||||
<Action ID="RbsAction" do="MoveToPose" command="move" sid="b"></Action>
|
||||
<Action ID="RbsAction" do="RecordingDemo" command="rdStop" sid="a"></Action>
|
||||
</Sequence>
|
||||
</BehaviorTree>
|
||||
</root>
|
73
test_examples/bt_rosbag/skills.json
Normal file
73
test_examples/bt_rosbag/skills.json
Normal file
|
@ -0,0 +1,73 @@
|
|||
{
|
||||
"skills": [
|
||||
{
|
||||
"sid": "a",
|
||||
"SkillPackage": { "name": "Robossembler", "version": "1.0", "format": "1" },
|
||||
"Module": { "name": "RecordingDemo", "description": "Recording a demo via RosBag", "node_name": "lc_record_demo" },
|
||||
"Launch": { "package": "rbs_utils", "executable": "recording_demo_via_rosbag.py" },
|
||||
"BTAction": [
|
||||
{
|
||||
"name": "rdConfigure",
|
||||
"type_action": "action",
|
||||
"type": "run",
|
||||
"param": []
|
||||
},
|
||||
{ "name": "rdStop", "type_action": "action", "type": "stop", "param": [] }
|
||||
],
|
||||
"topicsOut": [],
|
||||
"Settings": {
|
||||
"output": {
|
||||
"params": [
|
||||
{
|
||||
"name": "output_path",
|
||||
"value": "rbs_testbag"
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
},
|
||||
{
|
||||
"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.1, "z":0.7}, "orientation": {"x":0.55, "y":0.0, "z":0.45, "w": 1.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"
|
||||
}
|
||||
]
|
||||
}
|
||||
}
|
||||
}
|
||||
]
|
||||
}
|
Loading…
Add table
Add a link
Reference in a new issue