Full BT(moveit) with condition for checking Aruco

This commit is contained in:
shalenikol 2025-04-16 10:45:00 +03:00
parent eeaca5d72d
commit 2cdf4e91c7
3 changed files with 126 additions and 0 deletions

25
docs/to_start_BT_main.md Normal file
View file

@ -0,0 +1,25 @@
1) runtime CNC-Graver Assist
```sh
ros2 launch rbs_mill_assist simulation.launch.py
```
2) интерфейсная нода (bt_path: путь к skills.json)
```sh
ros2 launch rbs_bt_executor interface.launch.py bt_path:=/home/<user>/robossembler-ws/install/rbs_mill_assist/share/rbs_mill_assist/config/
```
3) BT
```sh
ros2 launch rbs_mill_assist bt_executor.launch.py
```
4) очередь
```sh
source ~/robossembler-ws/.venv/bin/activate
ros2 run rbs_mill_assist queue_srv.py
```
5) добавить задания в очередь (если их нет)
```sh
ros2 service call queue/add_tasks rbs_utils_interfaces/srv/AddTasks "{tasks_csv: /home/<user>/robossembler-ws/src/robossembler-cnc-graver-assist/docs/tasks.csv}"
```
6) запуск дерева поведения
```sh
ros2 action send_goal /behavior_server btcpp_ros2_interfaces/action/ExecuteTree "{target_tree: Main}"
```

View file

@ -10,6 +10,8 @@
<Sequence>
<!-- Preparing -->
<Action ID="RbssAction" do="ArucoPE" command="arucoConfig" sid="ArucoPE-example" action_name="rbs_interface_a"></Action>
<Action ID="TakeonTask" from_pose="{from_pose}" service_name="queue/takeon_task"/>
<!-- <ReactiveFallback>
<Action ID="TakeonTask" service_name="queue/takeon_task" from_pose="{from_pose}"/>
@ -48,6 +50,10 @@
service_name="/get_tf_frame_pose" />
<Action ID="MoveToPose" pose="{named_pose}" robot_name="arm" duration="3"
action_name="/mtp_jtc" />
<Inverter>
<Condition ID="RbssCondition" do="ArucoPE" command="isAruco" sid="ArucoPE-example" service_name="rbs_interface_s"/>
</Inverter>
<!-- Send task and wait Graver -->
@ -82,6 +88,8 @@
<Action ID="TaskCompleted" service_name="queue/task_completed"/>
<Action ID="RbssAction" do="ArucoPE" command="arucoStop" sid="ArucoPE-example" action_name="rbs_interface_a"></Action>
<!-- Ending -->
</Sequence>
</Repeat>

View file

@ -0,0 +1,93 @@
{
"skills": [
{
"sid": "ArucoPE-example",
"SkillPackage": { "name": "Robossembler", "version": "1.0", "format": "1" },
"Module": { "name": "ArucoPE", "node_name": "aruco_pe", "description": "Aruco Marker Tracking and Pose Estimation wrapper for ROS2, using RGB cameras" },
"Launch": { "package": "rbss_aruco_pe", "executable": "aruco_node_lc.py" },
"BTAction": [
{
"name": "arucoConfig",
"type": "run",
"param": [
{
"type": "topic",
"dependency": {
"type": "topic",
"topicType": "sensor_msgs/msg/Image",
"topicOut": "/rgbd_camera/image/image"
}
},
{
"type": "topic",
"dependency": {
"type": "topic",
"topicType": "sensor_msgs/msg/Image",
"topicOut": "/rgbd_camera/image/depth_image"
}
},
{
"type": "topic",
"dependency": {
"type": "topic",
"topicType": "sensor_msgs/msg/CameraInfo",
"topicOut": "/rgbd_camera/image/camera_info"
}
}
],
"typeAction": "ACTION"
},
{ "name": "isAruco", "type": "if", "typeAction": "CONDITION",
"param": [
{
"type": "formBuilder",
"dependency": {
"output": {
"aruco_id": "25"
}
}
}
]
},
{ "name": "arucoStop", "type": "stop", "param": [], "typeAction": "ACTION" }
],
"topicsOut": [
{
"name": "/aruco/markers",
"type": "rbs_skill_interfaces/msg/ArucoMarkers"
},
{
"name": "/aruco/poses",
"type": "geometry_msgs/msg/PoseArray"
},
{
"name": "/aruco/image",
"type": "sensor_msgs/msg/Image"
}
],
"Settings": {
"output": {
"params": [
{
"name": "is_image_mode",
"value": "True"
},
{
"name": "aruco_dictionary_id",
"value": "DICT_6X6_50"
},
{
"name": "marker_size",
"value": "0.04"
},
{
"name": "camera_frame",
"value": ""
}
]
},
"type": "formBuilder"
}
}
]
}