robossembler/runtime
7
0
Fork 0
Code Issues 4 Pull requests Projects Releases Packages Wiki Activity Actions

Migration to BT.cpp v4 & new BT executor

Browse source
This commit is contained in:
Ilya Uraev 2024-07-04 12:11:07 +00:00 • committed by Igor Brylyov
parent b58307dea1
commit 2de37b027b
69 changed files with 843 additions and 2065 deletions
Download patch file Download diff file Expand all files Collapse all files

9
rbs_bringup/launch/rbs_robot.launch.py
View file

@ -132,7 +132,8 @@ def launch_setup(context, *args, **kwargs):
robot_description_semantic,
robot_description_kinematics,
{'use_sim_time': use_sim_time}
]
],
condition=IfCondition(launch_rviz)
)
rviz = Node(
@ -231,11 +232,17 @@ def launch_setup(context, *args, **kwargs):
}.items(),
condition=IfCondition(launch_perception))
asm_config = Node(
package="rbs_utils",
executable="assembly_config_service.py"
)
nodes_to_start = [
robot_state_publisher,
control,
moveit,
skills,
asm_config,
# task_planner,
# perception,
rviz

6
rbs_bringup/launch/single_robot.launch.py
View file

@ -53,7 +53,7 @@ def launch_setup(context, *args, **kwargs):
launch_arguments={
'sim_gazebo': sim_gazebo,
'debugger': "false",
'launch_env_manager': "false",
'launch_env_manager': env_manager,
'gazebo_world_filename': "asm2.sdf"
}.items(),
condition=IfCondition(launch_simulation)
@ -224,7 +224,7 @@ def generate_launch_description():
)
declared_arguments.append(
DeclareLaunchArgument("with_gripper",
default_value="false",
default_value="true",
description="With gripper or not?")
)
declared_arguments.append(
@ -234,7 +234,7 @@ def generate_launch_description():
)
declared_arguments.append(
DeclareLaunchArgument("env_manager",
default_value="false",
default_value="true",
description="Launch env_manager?")
)
declared_arguments.append(

43
rbs_bt_executor/CMakeLists.txt
View file

@ -15,7 +15,8 @@ find_package(moveit_ros_planning REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(ament_index_cpp REQUIRED)
find_package(rbs_skill_interfaces REQUIRED)
find_package(behavior_tree REQUIRED)
# find_package(behavior_tree REQUIRED)
find_package(behaviortree_ros2 REQUIRED)
find_package(control_msgs REQUIRED)
find_package(lifecycle_msgs REQUIRED)
find_package(rcl_interfaces REQUIRED)
@ -38,7 +39,8 @@ set(dependencies
moveit_ros_planning_interface
ament_index_cpp
rbs_skill_interfaces
behavior_tree
# behavior_tree
behaviortree_ros2
control_msgs
lifecycle_msgs
rcl_interfaces
@ -53,23 +55,23 @@ include_directories(include)
add_library(rbs_skill_move_topose_bt_action_client SHARED src/MoveToPose.cpp)
list(APPEND plugin_libs rbs_skill_move_topose_bt_action_client)
add_library(rbs_skill_gripper_move_bt_action_client SHARED src/gripper_move.cpp)
add_library(rbs_skill_gripper_move_bt_action_client SHARED src/MoveGripper.cpp)
list(APPEND plugin_libs rbs_skill_gripper_move_bt_action_client)
add_library(rbs_skill_get_pick_place_pose_service_client SHARED src/GetPickPlacePoses.cpp)
list(APPEND plugin_libs rbs_skill_get_pick_place_pose_service_client)
# add_library(rbs_skill_get_pick_place_pose_service_client SHARED src/GetPickPlacePoses.cpp)
# list(APPEND plugin_libs rbs_skill_get_pick_place_pose_service_client)
add_library(rbs_skill_move_joint_state SHARED src/MoveToJointStates.cpp)
list(APPEND plugin_libs rbs_skill_move_joint_state)
add_library(rbs_add_planning_scene_object SHARED src/AddPlanningSceneObject.cpp)
list(APPEND plugin_libs rbs_add_planning_scene_object)
# add_library(rbs_add_planning_scene_object SHARED src/AddPlanningSceneObject.cpp)
# list(APPEND plugin_libs rbs_add_planning_scene_object)
add_library(rbs_assemble_process_state SHARED src/AssembleProcessState.cpp)
list(APPEND plugin_libs rbs_assemble_process_state)
# add_library(rbs_assemble_process_state SHARED src/AssembleProcessState.cpp)
# list(APPEND plugin_libs rbs_assemble_process_state)
add_library(rbs_pose_estimation SHARED src/PoseEstimation.cpp)
list(APPEND plugin_libs rbs_pose_estimation)
# add_library(rbs_pose_estimation SHARED src/PoseEstimation.cpp)
# list(APPEND plugin_libs rbs_pose_estimation)
add_library(rbs_object_detection SHARED src/ObjectDetection.cpp)
list(APPEND plugin_libs rbs_object_detection)
@ -80,8 +82,14 @@ list(APPEND plugin_libs rbs_env_manager_starter)
add_library(rbs_skill_move_topose_array_bt_action_client SHARED src/MoveToPoseArray.cpp)
list(APPEND plugin_libs rbs_skill_move_topose_array_bt_action_client)
add_library(rbs_interface SHARED src/rbsBTAction.cpp)
list(APPEND plugin_libs rbs_interface)
add_library(rbs_get_workspace SHARED src/GetWorkspace.cpp)
list(APPEND plugin_libs rbs_get_workspace)
add_executable(rbs_bt_executor src/TreeRunner.cpp)
ament_target_dependencies(rbs_bt_executor ${dependencies})
# add_library(rbs_interface SHARED src/rbsBTAction.cpp)
# list(APPEND plugin_libs rbs_interface)
foreach(bt_plugin ${plugin_libs})
ament_target_dependencies(${bt_plugin} ${dependencies})
@ -90,8 +98,17 @@ endforeach()
install(DIRECTORY launch bt_trees config DESTINATION share/${PROJECT_NAME})
# INSTAL PLUGIN TARGETS https://github.com/BehaviorTree/BehaviorTree.CPP/pull/804
install(TARGETS
${plugin_libs}
ARCHIVE DESTINATION share/${PROJECT_NAME}/bt_plugins
LIBRARY DESTINATION share/${PROJECT_NAME}/bt_plugins
RUNTIME DESTINATION share/${PROJECT_NAME}/bt_plugins
)
install(TARGETS
rbs_bt_executor
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION lib/${PROJECT_NAME}

21
rbs_bt_executor/bt_trees/PosesTest.xml
View file

@ -1,21 +0,0 @@
<?xml version="1.0"?>
<root main_tree_to_execute="GripperTree">
<BehaviorTree ID="GripperTree">
<Sequence>
<Action ID="GetPickPlacePoses"
ObjectName="box1"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
server_name="/get_pick_place_pose_service"
server_timeout="1000"/>
</Sequence>
</BehaviorTree>
</root>

57
rbs_bt_executor/bt_trees/assemble.xml
View file

@ -1,57 +0,0 @@
<?xml version="1.0"?>
<root main_tree_to_execute="Assemble">
<BehaviorTree ID="Assemble">
<Sequence>
<Action ID="DetectObject" ReqKind="FOLLOW" ObjectName="${arg0}"
server_name="/ground_true/scene_component" server_timeout="1000"/>
<Action ID="AssembleState" StateKind="INITIALIZE" PartName="${arg0}"
AssembleName="${arg2}" WorkspaceName="${arg3}" server_name="/assemble_state"
server_timeout="1000"/>
<Action ID="GetPickPlacePoses" ObjectName="${arg0}" GraspDirection = "y" PlaceDirection = "z"
GraspPose="{GraspPose}" PostGraspPose="{PostGraspPose}" PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}" PlacePose="{PlacePose}" PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}" server_name="/get_pick_place_pose_service" server_timeout="1000"/>
<Action ID="MoveToPose" robot_name="${arg4}" pose="{PreGraspPose}"
server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="${arg4}" pose="{GraspPose}"
server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control"
server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="${arg4}" pose="{PostGraspPose}"
server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="${arg4}" pose="{PostPostGraspPose}"
server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="${arg4}" pose="{PrePlacePose}"
server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="${arg4}" pose="{PlacePose}"
server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control"
server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="${arg4}" pose="{PostPlacePose}"
server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
`
<Action ID="AssembleState" StateKind="VALIDATE" PartName="${arg0}"
AssembleName="${arg2}" WorkspaceName="${arg3}" server_name="/assemble_state"
server_timeout="1000"/>
<Action ID="AssembleState" StateKind="COMPLETE" PartName="${arg0}"
AssembleName="${arg2}" WorkspaceName="${arg3}" server_name="/assemble_state"
server_timeout="1000"/>
<Action ID="DetectObject" ReqKind="CANCEL" ObjectName="${arg0}"
server_name="/ground_true/scene_component" server_timeout="1000"/>
</Sequence>
</BehaviorTree>
</root>

20
rbs_bt_executor/bt_trees/atomic_skills_xml/assemble.xml
View file

@ -1,5 +1,5 @@
<?xml version="1.0"?>
<root main_tree_to_execute="Assemble">
<?xml version='1.0' encoding='utf-8'?>
<root main_tree_to_execute="Assemble" BTCPP_format="4">
<!-- ////////// -->
<BehaviorTree ID="Assemble">
<Sequence>
@ -15,20 +15,20 @@
<!-- ////////// -->
<TreeNodesModel>
<Action ID="GetGraspPoses">
<input_port name="frame"/>
<output_port name="grasp_pose"/>
<input_port name="frame" />
<output_port name="grasp_pose" />
</Action>
<Action ID="MoveToPose">
<input_port name="arm_group"/>
<input_port name="command"/>
<input_port name="arm_group" />
<input_port name="command" />
</Action>
<Action ID="MoveGripper">
<input_port name="arm_group"/>
<input_port name="command"/>
<input_port name="arm_group" />
<input_port name="command" />
</Action>
<Action ID="Print">
<input_port name="frame"/>
<input_port name="frame" />
</Action>
</TreeNodesModel>
<!-- ////////// -->
</root>
</root>

37
rbs_bt_executor/bt_trees/atomic_skills_xml/get-part-state.xml
View file

@ -1,40 +1,39 @@
<?xml version="1.0"?>
<root main_tree_to_execute="GraspPart">
<?xml version='1.0' encoding='utf-8'?>
<root main_tree_to_execute="GraspPart" BTCPP_format="4">
<!-- ////////// -->
<BehaviorTree ID="GraspPart">
<Fallback name="root_Fallback">
<Sequence name="GraspSkill">
<Fallback>
<Condition ID="PartInScene"/>
<Action ID="FindPart" PartName="{arg1}" PartPose="{arg0}"/>
<Condition ID="PartInScene" />
<Action ID="FindPart" PartName="{arg1}" PartPose="{arg0}" />
</Fallback>
<Action ID="MoveToPoint" arm_group="${arg2}" goal="${arg0}"/>
<Action ID="GraspPart" arm_group="${arg3}" fingers_wide="${arg4}"/>
<Action ID="MoveToPoint" arm_group="{arg2}" goal="{arg5}"/>
<Action ID="MoveToPoint" arm_group="${arg2}" goal="${arg0}" />
<Action ID="GraspPart" arm_group="${arg3}" fingers_wide="${arg4}" />
<Action ID="MoveToPoint" arm_group="{arg2}" goal="{arg5}" />
</Sequence>
</Fallback>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="FindPart">
<inout_port name="PartName"/>
<output_port name="PartPose"/>
<inout_port name="PartName" />
<output_port name="PartPose" />
</Action>
<Action ID="GraspPart">
<input_port name="arm_group"/>
<input_port name="fingers_wide"/>
<input_port name="arm_group" />
<input_port name="fingers_wide" />
</Action>
<Action ID="MoveToPoint">
<input_port name="arm_group"/>
<input_port name="goal"/>
<input_port name="arm_group" />
<input_port name="goal" />
</Action>
<Condition ID="PartInScene"/>
<Condition ID="PathFree"/>
<Condition ID="PartInScene" />
<Condition ID="PathFree" />
<Action ID="SpawnPart">
<inout_port name="PartName"/>
<input_port name="PartPose"/>
<inout_port name="PartName" />
<input_port name="PartPose" />
</Action>
</TreeNodesModel>
<!-- ////////// -->
</root>
</root>

37
rbs_bt_executor/bt_trees/atomic_skills_xml/grasp_part.xml
View file

@ -1,42 +1,41 @@
<?xml version="1.0"?>
<root main_tree_to_execute="GraspPart">
<?xml version='1.0' encoding='utf-8'?>
<root main_tree_to_execute="GraspPart" BTCPP_format="4">
<!-- ////////// -->
<BehaviorTree ID="GraspPart">
<Fallback name="root_Fallback">
<Sequence name="GraspSkill">
<ReactiveFallback name="PartEstimate">
<Condition ID="PartInScene"/>
<Action ID="FindPart" PartName="{arg1}" PartPose="{arg0}"/>
<Condition ID="PartInScene" />
<Action ID="FindPart" PartName="{arg1}" PartPose="{arg0}" />
</ReactiveFallback>
<Fallback name="Move">
<Condition ID="PathFree"/>
<Action ID="MoveToPoint" arm_group="${arg2}" goal="${arg0}"/>
<Condition ID="PathFree" />
<Action ID="MoveToPoint" arm_group="${arg2}" goal="${arg0}" />
</Fallback>
<Action ID="GraspPart" arm_group="${arg3}"/>
<Action ID="MoveToPoint" arm_group="{arg2}" goal="{arg4}"/>
<Action ID="GraspPart" arm_group="${arg3}" />
<Action ID="MoveToPoint" arm_group="{arg2}" goal="{arg4}" />
</Sequence>
</Fallback>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="FindPart">
<inout_port name="PartName"/>
<output_port name="PartPose"/>
<inout_port name="PartName" />
<output_port name="PartPose" />
</Action>
<Action ID="GraspPart">
<input_port name="arm_group"/>
<input_port name="arm_group" />
</Action>
<Action ID="MoveToPoint">
<input_port name="arm_group"/>
<input_port name="goal"/>
<input_port name="arm_group" />
<input_port name="goal" />
</Action>
<Condition ID="PartInScene"/>
<Condition ID="PathFree"/>
<Condition ID="PartInScene" />
<Condition ID="PathFree" />
<Action ID="SpawnPart">
<inout_port name="PartName"/>
<input_port name="PartPose"/>
<inout_port name="PartName" />
<input_port name="PartPose" />
</Action>
</TreeNodesModel>
<!-- ////////// -->
</root>
</root>

38
rbs_bt_executor/bt_trees/atomic_skills_xml/grasp_skill.xml
View file

@ -1,38 +1,38 @@
<?xml version="1.0"?>
<root main_tree_to_execute="GraspPart">
<?xml version='1.0' encoding='utf-8'?>
<root main_tree_to_execute="GraspPart" BTCPP_format="4">
<!-- ////////// -->
<BehaviorTree ID="GraspPart">
<Sequence name="GraspSkill">
<Action ID="SpawnPart" PartName="${arg2}" PartPose="${gazebo_entity_name}"/>
<Action ID="SpawnPart" PartName="${arg2}" PartPose="${gazebo_entity_name}" />
<Sequence>
<Condition ID="PartInScene"/>
<Action ID="FindPart" PartName="${arg2}" PartPose="${part_pose}"/>
<Condition ID="PartInScene" />
<Action ID="FindPart" PartName="${arg2}" PartPose="${part_pose}" />
</Sequence>
<Action ID="Move" arm_group="${arg0}" goal="${part_pose}"/>
<Action ID="GraspPart" arm_group="${arg1}" fingers_wide="${goal1}"/>
<Action ID="Move" arm_group="${arg0}" goal="${goal2}"/>
<Action ID="Move" arm_group="${arg0}" goal="${part_pose}" />
<Action ID="GraspPart" arm_group="${arg1}" fingers_wide="${goal1}" />
<Action ID="Move" arm_group="${arg0}" goal="${goal2}" />
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="FindPart">
<inout_port name="PartName"/>
<output_port name="PartPose"/>
<inout_port name="PartName" />
<output_port name="PartPose" />
</Action>
<Action ID="GraspPart">
<input_port name="arm_group"/>
<input_port name="fingers_wide"/>
<input_port name="arm_group" />
<input_port name="fingers_wide" />
</Action>
<Action ID="Move">
<input_port name="arm_group"/>
<input_port name="goal"/>
<input_port name="arm_group" />
<input_port name="goal" />
</Action>
<Condition ID="PartInScene"/>
<Condition ID="PathFree"/>
<Condition ID="PartInScene" />
<Condition ID="PathFree" />
<Action ID="SpawnPart">
<inout_port name="PartName"/>
<input_port name="PartPose"/>
<inout_port name="PartName" />
<input_port name="PartPose" />
</Action>
</TreeNodesModel>
<!-- ////////// -->
</root>
</root>

5
rbs_bt_executor/bt_trees/atomic_skills_xml/move.xml
View file

@ -1,7 +1,8 @@
<root main_tree_to_execute = "MainTree" >
<?xml version='1.0' encoding='utf-8'?>
<root main_tree_to_execute="MainTree" BTCPP_format="4">
<BehaviorTree ID="MainTree">
<Sequence name="root_sequence">
<Move name="move" arm_group="${arg0}" goal="${arg1}"/>
<Move name="move" arm_group="${arg0}" goal="${arg1}" />
</Sequence>
</BehaviorTree>
</root>

5
rbs_bt_executor/bt_trees/atomic_skills_xml/move_gripper.xml
View file

@ -1,7 +1,8 @@
<root main_tree_to_execute = "MainTree" >
<?xml version='1.0' encoding='utf-8'?>
<root main_tree_to_execute="MainTree" BTCPP_format="4">
<BehaviorTree ID="MainTree">
<Sequence name="root_sequence">
<MoveGripper name="move_gripper" gripper_group="${arg0}" relative_gap="${arg1}"/>
<MoveGripper name="move_gripper" gripper_group="${arg0}" relative_gap="${arg1}" />
</Sequence>
</BehaviorTree>
</root>

8
rbs_bt_executor/bt_trees/atomic_skills_xml/print.xml
View file

@ -1,5 +1,5 @@
<?xml version="1.0"?>
<root main_tree_to_execute="Print">
<?xml version='1.0' encoding='utf-8'?>
<root main_tree_to_execute="Print" BTCPP_format="4">
<!-- ////////// -->
<BehaviorTree ID="Print">
<Sequence>
@ -9,8 +9,8 @@
<!-- ////////// -->
<TreeNodesModel>
<Action ID="Print">
<input_port name="frame"/>
<input_port name="printer"/>
<input_port name="frame" />
<input_port name="printer" />
</Action>
</TreeNodesModel>
<!-- ////////// -->

137
rbs_bt_executor/bt_trees/atomic_skills_xml/remove.xml
View file

@ -1,106 +1,31 @@
<?xml version="1.0"?>
<root main_tree_to_execute="Remove">
<?xml version='1.0' encoding='utf-8'?>
<root main_tree_to_execute="Remove" BTCPP_format="4">
<!-- ////////// -->
<BehaviorTree ID="Remove">
<Sequence>
<Action ID="GetGraspPoses"
part_id="${arg0}"
grasp_pose="{grasp_pose}"
pre_grasp_pose="{pre_grasp_pose}"
pre_gap_distance="{pre_gap_distance}"
gap_distance="{gap_distance}"
server_name="/scene_monitor/get_grasp_poses"
server_timeout="10" />
<Action ID="GetGraspPoses" part_id="${arg0}" grasp_pose="{grasp_pose}" pre_grasp_pose="{pre_grasp_pose}" pre_gap_distance="{pre_gap_distance}" gap_distance="{gap_distance}" server_name="/scene_monitor/get_grasp_poses" server_timeout="10" />
<Action ID="MoveToPose"
robot_name="${arg3}"
pose="{pre_grasp_pose}"
<Action ID="MoveToPose" robot_name="${arg3}" pose="{pre_grasp_pose}" server_name="/move_topose" server_timeout="10" cancel_timeout="5" />
server_name="/move_topose"
server_timeout="10"
cancel_timeout="5" />
<Action ID="GripperCmd" gap_distance="{pre_gap_distance}" frame_name="${arg0}" server_name="/gripper_cmd" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperCmd"
gap_distance="{pre_gap_distance}"
frame_name="${arg0}"
server_name="/gripper_cmd"
server_timeout="10"
cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="${arg3}" pose="{grasp_pose}" server_name="/move_topose" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose"
robot_name="${arg3}"
pose="{grasp_pose}"
<Action ID="GripperCmd" gap_distance="{gap_distance}" frame_name="${arg0}" server_name="/gripper_cmd" server_timeout="10" cancel_timeout="5" />
server_name="/move_topose"
server_timeout="10"
cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="${arg3}" pose="{pre_grasp_pose}" server_name="/move_topose" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperCmd"
gap_distance="{gap_distance}"
frame_name="${arg0}"
server_name="/gripper_cmd"
server_timeout="10"
cancel_timeout="5" />
<Action ID="ConstructWorkspacePlacementPose" part_id="${arg0}" workspace="${arg2}" constructed_part="constructed_part" server_name="/scene_monitor/construct_workspace_placement_pose" server_timeout="10" />
<Action ID="MoveToPose"
robot_name="${arg3}"
pose="{pre_grasp_pose}"
<Action ID="GetWorkspacePlacementPose" part_id="{constructed_part}" workspace="${arg2}" pre_placement_pose="{pre_placement_pose}" placement_pose="{placement_pose}" server_name="/scene_monitor/get_workspace_placement_pose" server_timeout="10" />
server_name="/move_topose"
server_timeout="10"
cancel_timeout="5" />
<Action ID="ConstructWorkspacePlacementPose"
part_id="${arg0}"
workspace="${arg2}"
constructed_part="constructed_part"
server_name="/scene_monitor/construct_workspace_placement_pose"
server_timeout="10"/>
<Action ID="GetWorkspacePlacementPose"
part_id="{constructed_part}"
workspace="${arg2}"
pre_placement_pose="{pre_placement_pose}"
placement_pose="{placement_pose}"
server_name="/scene_monitor/get_workspace_placement_pose"
server_timeout="10" />
<Action ID="MoveToPose"
robot_name="${arg3}"
pose="{pre_placement_pose}"
server_name="/move_topose"
server_timeout="10"
cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="${arg3}" pose="{pre_placement_pose}" server_name="/move_topose" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose"
robot_name="${arg3}"
pose="{placement_pose}"
server_name="/move_topose"
server_timeout="10"
cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="${arg3}" pose="{placement_pose}" server_name="/move_topose" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperCmd"
gap_distance="{pre_gap_distance}"
frame_name="${arg0}"
server_name="/gripper_cmd"
server_timeout="10"
cancel_timeout="5" />
<Action ID="GripperCmd" gap_distance="{pre_gap_distance}" frame_name="${arg0}" server_name="/gripper_cmd" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose"
robot_name="${arg3}"
pose="{pre_placement_pose}"
server_name="/move_topose"
server_timeout="10"
cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="${arg3}" pose="{pre_placement_pose}" server_name="/move_topose" server_timeout="10" cancel_timeout="5" />
</Sequence>
</BehaviorTree>
@ -108,37 +33,37 @@
<TreeNodesModel>
<Action ID="GetGraspPoses">
<input_port name="part_id"/>
<input_port name="part_id" />
<output_port name="grasp_pose"/>
<output_port name="pre_grasp_pose"/>
<output_port name="pre_gap_distance"/>
<output_port name="gap_distance"/>
<output_port name="grasp_pose" />
<output_port name="pre_grasp_pose" />
<output_port name="pre_gap_distance" />
<output_port name="gap_distance" />
</Action>
<Action ID="MoveToPose">
<input_port name="robot_name"/>
<input_port name="pose"/>
<input_port name="robot_name" />
<input_port name="pose" />
</Action>
<Action ID="GripperCmd">
<input_port name="gap_distance"/>
<input_port name="frame_name"/>
<input_port name="gap_distance" />
<input_port name="frame_name" />
</Action>
<Action ID="ConstructWorkspacePlacementPose">
<input_port name="part_id"/>
<input_port name="workspace"/>
<output_port name="constructed_part"/>
<input_port name="part_id" />
<input_port name="workspace" />
<output_port name="constructed_part" />
</Action>
<Action ID="GetWorkspacePlacementPose">
<input_port name="part_id"/>
<input_port name="workspace"/>
<output_port name="pre_placement_pose"/>
<output_port name="placement_pose"/>
<input_port name="part_id" />
<input_port name="workspace" />
<output_port name="pre_placement_pose" />
<output_port name="placement_pose" />
</Action>
</TreeNodesModel>
<!-- ////////// -->
</root>
</root>

17
rbs_bt_executor/bt_trees/bt_movetopose.xml
View file

@ -1,17 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<root main_tree_to_execute="Main">
<BehaviorTree ID="Main">
<Sequence>
<Action ID="RbsBtAction" do="MoveToPose" command="move" server_name="rbs_interface" server_timeout="1000" />
</Sequence>
</BehaviorTree>
<TreeNodesModel>
<Action ID="RbsBtAction">
<input_port name="do" />
<input_port name="command" />
<input_port name="server_name" />
<input_port name="server_timeout" />
</Action>
</TreeNodesModel>
</root>

18
rbs_bt_executor/bt_trees/bt_pe_stop.xml
View file

@ -1,18 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<root main_tree_to_execute="Main">
<BehaviorTree ID="Main">
<Sequence>
<Action ID="RbsBtAction" sid="124" do="PoseEstimation" command="peStop" server_name="rbs_interface" server_timeout="1000" />
</Sequence>
</BehaviorTree>
<TreeNodesModel>
<Action ID="RbsBtAction">
<input_port name="sid" />
<input_port name="do" />
<input_port name="command" />
<input_port name="server_name" />
<input_port name="server_timeout" />
</Action>
</TreeNodesModel>
</root>

18
rbs_bt_executor/bt_trees/bt_pe_test.xml
View file

@ -1,18 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<root main_tree_to_execute="Main">
<BehaviorTree ID="Main">
<Sequence>
<Action ID="RbsBtAction" sid="123" do="PoseEstimation" command="peConfigure" server_name="rbs_interface" server_timeout="1000" />
</Sequence>
</BehaviorTree>
<TreeNodesModel>
<Action ID="RbsBtAction">
<input_port name="sid" />
<input_port name="do" />
<input_port name="command" />
<input_port name="server_name" />
<input_port name="server_timeout" />
</Action>
</TreeNodesModel>
</root>

22
rbs_bt_executor/bt_trees/bt_template.xml
View file

@ -1,22 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<root main_tree_to_execute="Main">
<BehaviorTree ID="Main">
<Sequence>
<Action ID="RbsBtAction" do="PoseEstimation" command="peConfigure" server_name="rbs_interface" server_timeout="1000" />
<Action ID="RbsBtAction" do="PoseEstimation" command="peStop" server_name="rbs_interface" server_timeout="1000" />
</Sequence>
</BehaviorTree>
<TreeNodesModel>
<Action ID="RbsBtAction">
<input_port name="do" />
<input_port name="command" />
<input_port name="server_name" />
<input_port name="server_timeout" />
</Action>
</TreeNodesModel>
<!-- <Action ID="RbsBtAction" do="ObjectDetection" command="odConfigure" server_name="rbs_interface" server_timeout="1000" />
<Action ID="RbsBtAction" do="ObjectDetection" command="odStop" server_name="rbs_interface" server_timeout="1000" /> -->
<!-- <input_port name="cancel_timeout" /> -->
<!-- cancel_timeout="1000" -->
</root>

19
rbs_bt_executor/bt_trees/example_repeat.xml
View file

@ -1,19 +0,0 @@
<?xml version="1.0"?>
<root main_tree_to_execute="BehaviorTree">
<!-- ////////// -->
<BehaviorTree ID="BehaviorTree">
<Sequence>
<SetBlackboard output_key="" value=""/>
<Repeat num_cycles="3">
<Sequence>
<AlwaysSuccess/>
<AlwaysSuccess/>
<AlwaysFailure/>
</Sequence>
</Repeat>
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel/>
<!-- ////////// -->
</root>

11
rbs_bt_executor/bt_trees/example_skills/env_manager_starter.xml
View file

@ -1,11 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<root main_tree_to_execute="Main">
<Action ID="EnvStarter"
env_class="gz_enviroment::GzEnviroment"
env_name="gz_enviroment"
server_name="/env_manager/start_env"
server_timeout="1000"
workspace="{workspace}" />
<?xml version='1.0' encoding='utf-8'?>
<root main_tree_to_execute="Main" BTCPP_format="4">
<Action ID="EnvStarter" env_class="gz_enviroment::GzEnviroment" env_name="gz_enviroment" server_name="/env_manager/start_env" server_timeout="1000" workspace="{workspace}" />
<TreeNodesModel>
<Action ID="EnvStarter">

13
rbs_bt_executor/bt_trees/example_skills/move_to_joint_states.xml
View file

@ -1,11 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<root main_tree_to_execute="Main">
<Action ID="MoveToPose"
robot_name="{robot_name}"
JointState="{joint_state}"
server_name="{action_server}"
server_timeout="{time_s}"
cancel_timeout="{time_c}" />
<?xml version='1.0' encoding='utf-8'?>
<root main_tree_to_execute="Main" BTCPP_format="4">
<Action ID="MoveToPose" robot_name="{robot_name}" JointState="{joint_state}" server_name="{action_server}" server_timeout="{time_s}" cancel_timeout="{time_c}" />
<TreeNodesModel>
<Action ID="MoveToPose">
@ -20,4 +15,4 @@
<input_port name="server_timeout" />
</Action>
</TreeNodesModel>
</root>
</root>

13
rbs_bt_executor/bt_trees/example_skills/move_to_pose.xml
View file

@ -1,11 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<root main_tree_to_execute="Main">
<Action ID="MoveToPose"
robot_name="{robot_name}"
pose="{pose}"
server_name="{action_server}"
server_timeout="{time_s}"
cancel_timeout="{time_c}" />
<?xml version='1.0' encoding='utf-8'?>
<root main_tree_to_execute="Main" BTCPP_format="4">
<Action ID="MoveToPose" robot_name="{robot_name}" pose="{pose}" server_name="{action_server}" server_timeout="{time_s}" cancel_timeout="{time_c}" />
<TreeNodesModel>
<Action ID="MoveToPose">
@ -20,4 +15,4 @@
<input_port name="server_timeout" />
</Action>
</TreeNodesModel>
</root>
</root>

14
rbs_bt_executor/bt_trees/example_skills/move_to_pose_array.xml
View file

@ -1,12 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<root main_tree_to_execute="Main">
<Action ID="MoveToPoseArray"
cancel_timeout="5000"
pose_vec_in="{goal_pose}"
pose_vec_out="{goal_pose}"
robot_name="{robot_name}"
server_name="{server_name}"
server_timeout="5000" />
<?xml version='1.0' encoding='utf-8'?>
<root main_tree_to_execute="Main" BTCPP_format="4">
<Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}" pose_vec_out="{goal_pose}" robot_name="{robot_name}" server_name="{server_name}" server_timeout="5000" />
<TreeNodesModel>
<Action ID="MoveToPose">
<!-- Непосредственно сама позиция в формате ROS2 сообщения-->
@ -23,4 +17,4 @@
<input_port name="server_timeout" />
</Action>
</TreeNodesModel>
</root>
</root>

30
rbs_bt_executor/bt_trees/find_object.xml
View file

@ -1,30 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<root BTCPP_format="4">
<BehaviorTree ID="Untitled">
<Sequence>
<SetBlackboard name="InitBT"
value="param_name"
output_key="param_values"/>
<Fallback>
<ReactiveSequence>
<GoToPose PoseArray="{param}"
robot_name="{robot_name}"/>
<AproachObject/>
</ReactiveSequence>
<AlwaysSuccess name="DoSomethingElse"/>
</Fallback>
</Sequence>
</BehaviorTree>
<!-- Description of Node Models (used by Groot) -->
<TreeNodesModel>
<Condition ID="AproachObject"
editable="true"/>
<Action ID="GoToPose"
editable="true">
<input_port name="PoseArray">Load from outside</input_port>
<input_port name="robot_name"/>
</Action>
</TreeNodesModel>
</root>

3
rbs_bt_executor/bt_trees/nodes_interfaces/general.xml
View file

@ -1,4 +1,5 @@
<root>
<?xml version='1.0' encoding='utf-8'?>
<root BTCPP_format="4">
<TreeNodesModel>
<Action ID="EnvStarter">
<input_port name="env_class" />

61
rbs_bt_executor/bt_trees/pick_and_place_modified_test_xml.xml
View file

@ -1,61 +0,0 @@
<?xml version="1.0"?>
<root main_tree_to_execute="BehaviorTree">
<!-- ////////// -->
<BehaviorTree ID="BehaviorTree">
<Sequence>
<Action GraspPose="GraspPose" ID="GetPickPlacePoses" ObjectName="ObjectPose" PlacePose="PlacePose" PostGraspPose="PostGraspPose" PostPlacePose="PostPlacePose" PreGraspPose="PreGraspPose" PrePlacePose="PrePlacePose"/>
<ReactiveSequence>
<Sequence>
<SubTree ID="Pick"/>
<SubTree ID="Place"/>
</Sequence>
<Action ID="SetNextObject" new_object="ObjectName"/>
<Action GraspPose="GraspPose" ID="GetPickPlacePoses" ObjectName="ObjectPose" PlacePose="PlacePose" PostGraspPose="PostGraspPose" PostPlacePose="PostPlacePose" PreGraspPose="PreGraspPose" PrePlacePose="PrePlacePose"/>
</ReactiveSequence>
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<BehaviorTree ID="Pick">
<Sequence>
<Action ID="MoveToPose" motion_type="ptp" pose="PreGraspPose"/>
<Action ID="GripperCommand" command="open"/>
<Action ID="MoveToPose" motion_type="cp" pose="GraspPosition"/>
<Action ID="GripperCommand" command="close"/>
<Action ID="MoveToPose" motion_type="cp" pose="PostGraspPose"/>
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<BehaviorTree ID="Place">
<Sequence>
<Action ID="MoveToPose" motion_type="ptp" pose="PrePlacePose"/>
<Action ID="MoveToPose" motion_type="cp" pose="PlacePose"/>
<Action ID="GripperCommand" command="open"/>
<Action ID="MoveToPose" motion_type="cp" pose="PostPlacePose"/>
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="GetPickPlacePoses">
<output_port default="GraspPose" name="GraspPose"/>
<inout_port default="ObjectPose" name="ObjectName"/>
<output_port default="PlacePose" name="PlacePose"/>
<output_port default="PostGraspPose" name="PostGraspPose"/>
<output_port default="PostPlacePose" name="PostPlacePose"/>
<output_port default="PreGraspPose" name="PreGraspPose"/>
<output_port default="PrePlacePose" name="PrePlacePose"/>
</Action>
<Action ID="GripperCommand">
<input_port default="open" name="command"/>
</Action>
<Action ID="MoveToPose">
<input_port default="cartesian" name="motion_type"/>
<input_port default="PreGraspPose" name="pose"/>
</Action>
<SubTree ID="Pick"/>
<SubTree ID="Place"/>
<Action ID="SetNextObject">
<inout_port name="new_object"/>
</Action>
</TreeNodesModel>
<!-- ////////// -->
</root>

50
rbs_bt_executor/bt_trees/pick_and_place_test.xml
View file

@ -1,50 +0,0 @@
<?xml version="1.0"?>
<root main_tree_to_execute="BehaviorTree">
<!-- ////////// -->
<BehaviorTree ID="BehaviorTree">
<ReactiveSequence>
<SubTree ID="Pick"/>
<SubTree ID="Place"/>
</ReactiveSequence>
</BehaviorTree>
<!-- ////////// -->
<BehaviorTree ID="Pick">
<Sequence>
<Action ID="GetPickPlacePoses" ObjectPose="ObjectPose" Pose="{GraspPose}" PostPose="{PostGraspPose}" PrePose="{PreGraspPose}" name="box1"/>
<Action ID="MoveToPose" motion_type="ptp" pose="PreGraspPose"/>
<Action ID="GripperCommand" command="open"/>
<Action ID="MoveToPose" motion_type="cp" pose="GraspPosition"/>
<Action ID="GripperCommand" command="close"/>
<Action ID="MoveToPose" motion_type="cp" pose="PostGraspPose"/>
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<BehaviorTree ID="Place">
<Sequence>
<Action ID="GetPickPlacePoses" ObjectPose="ObjectPose" Pose="PlacePose" PostPose="PostPlacePose" PrePose="PrePlacePose" name="GetPlacePoses"/>
<Action ID="MoveToPose" motion_type="ptp" pose="PrePlacePose"/>
<Action ID="MoveToPose" motion_type="cp" pose="PlacePose"/>
<Action ID="GripperCommand" command="open"/>
<Action ID="MoveToPose" motion_type="cp" pose="PostPlacePose"/>
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<TreeNodesModel>
<Action ID="GetPickPlacePoses">
<input_port default="ObjectPose" name="ObjectPose"/>
<output_port default="GraspPose" name="Pose"/>
<output_port default="PostGraspPose" name="PostPose"/>
<output_port default="PreGraspPose" name="PrePose"/>
</Action>
<Action ID="GripperCommand">
<input_port default="open" name="command"/>
</Action>
<Action ID="MoveToPose">
<input_port default="cartesian" name="motion_type"/>
<input_port default="PreGraspPose" name="pose"/>
</Action>
<SubTree ID="Pick"/>
<SubTree ID="Place"/>
</TreeNodesModel>
<!-- ////////// -->
</root>

14
rbs_bt_executor/bt_trees/test_gripper.xml
View file

@ -1,14 +0,0 @@
<?xml version="1.0"?>
<root main_tree_to_execute="MainTree">
<BehaviorTree ID="MainTree">
<Sequence>
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
</Sequence>
</BehaviorTree>
</root>

17
rbs_bt_executor/bt_trees/test_objdet_cleanup.xml
View file

@ -1,17 +0,0 @@
<?xml version="1.0"?>
<root main_tree_to_execute="ObjDetection">
<BehaviorTree ID="ObjDetection">
<Sequence>
<Action ID="ObjectDetection"
SettingPath=""
ReqKind = "deactivate"
server_name="/object_detection/change_state"
server_timeout="1000"/>
<Action ID="ObjectDetection"
SettingPath=""
ReqKind = "cleanup"
server_name="/object_detection/change_state"
server_timeout="1000"/>
</Sequence>
</BehaviorTree>
</root>

17
rbs_bt_executor/bt_trees/test_objdet_run.xml
View file

@ -1,17 +0,0 @@
<?xml version="1.0"?>
<root main_tree_to_execute="ObjDetection">
<BehaviorTree ID="ObjDetection">
<Sequence>
<Action ID="ObjectDetection"
SettingPath="!/home/shalenikol/0yolov8/test.json"
ReqKind = "configure"
server_name="/object_detection/change_state"
server_timeout="1000"/>
<Action ID="ObjectDetection"
SettingPath=""
ReqKind = "activate"
server_name="/object_detection/change_state"
server_timeout="1000"/>
</Sequence>
</BehaviorTree>
</root>

55
rbs_bt_executor/bt_trees/test_roboclone.xml
View file

@ -1,36 +1,27 @@
<?xml version="1.0"?>
<root main_tree_to_execute="MainTree">
<!-- ////////// -->
<BehaviorTree ID="MainTree">
<?xml version='1.0' encoding='utf-8'?>
<root BTCPP_format="4">
<BehaviorTree ID="InspectWorkspaceTwoArms">
<Sequence>
<Action ID="EnvStarter" env_class="gz_enviroment::GzEnviroment" env_name="gz_enviroment"
server_name="/env_manager/start_env" server_timeout="1000" workspace="{workspace}" />
<SubTreePlus ID="WorkspaceInspection" __autoremap="1" goal_pose="{workspace}"
robot_name="rbs_arm" />
<Action ID="EnvStarter" env_class="gz_enviroment::GzEnviroment" env_name="gz_enviroment"
service_name="/env_manager/start_env" msec="1000" workspace="{workspace}" />
<SubTree ID="WorkspaceInspection" goal_pose="{workspace}" robot_name="rbs_arm" _autoremap="1" />
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<BehaviorTree ID="WorkspaceInspection">
<Sequence>
<Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
pose_vec_out="{goal_pose}"
robot_name="{robot_name}" server_name="/arm1/move_topose" server_timeout="5000" />
<Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
pose_vec_out="{goal_pose}"
robot_name="{robot_name}" server_name="/arm1/move_cartesian" server_timeout="5000" />
<Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
pose_vec_out="{goal_pose}"
robot_name="{robot_name}" server_name="/arm1/move_cartesian" server_timeout="5000" />
<Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
pose_vec_out="{goal_pose}"
robot_name="{robot_name}" server_name="/arm1/move_cartesian" server_timeout="5000" />
<Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
pose_vec_out="{goal_pose}"
robot_name="{robot_name}" server_name="/arm/move_cartesian" server_timeout="5000" />
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<include path="./nodes_interfaces/general.xml"/>
<!-- ////////// -->
</root>
<!-- <BehaviorTree ID="WorkspaceInspection"> -->
<!-- <Sequence> -->
<!-- <Action ID="MoveToPoseArray" pose_vec_in="{goal_pose}" pose_vec_out="{goal_pose}" -->
<!-- robot_name="{robot_name}" action_name="/move_topose" msec="5000" /> -->
<!-- <Action ID="MoveToPoseArray" pose_vec_in="{goal_pose}" pose_vec_out="{goal_pose}" -->
<!-- robot_name="{robot_name}" action_name="/move_cartesian" msec="5000" /> -->
<!-- <Action ID="MoveToPoseArray" pose_vec_in="{goal_pose}" pose_vec_out="{goal_pose}" -->
<!-- robot_name="{robot_name}" action_name="/move_cartesian" msec="5000" /> -->
<!-- <Action ID="MoveToPoseArray" pose_vec_in="{goal_pose}" pose_vec_out="{goal_pose}" -->
<!-- robot_name="{robot_name}" action_name="/move_cartesian" msec="5000" /> -->
<!-- <Action ID="MoveToPoseArray" pose_vec_in="{goal_pose}" pose_vec_out="{goal_pose}" -->
<!-- robot_name="{robot_name}" action_name="/move_cartesian" msec="5000" /> -->
<!-- </Sequence> -->
<!-- </BehaviorTree> -->
<!-- <include path="./nodes_interfaces/general.xml" /> -->
</root>

41
rbs_bt_executor/bt_trees/test_tree.xml
View file

@ -1,36 +1,11 @@
<?xml version="1.0"?>
<root main_tree_to_execute="MainTree">
<!-- ////////// -->
<BehaviorTree ID="MainTree">
<?xml version='1.0' encoding='utf-8'?>
<root BTCPP_format="4">
<BehaviorTree ID="TestTree">
<Sequence>
<Action ID="EnvStarter" env_class="gz_enviroment::GzEnviroment" env_name="gz_enviroment"
server_name="/env_manager/start_env" server_timeout="1000" workspace="{workspace}" />
<SubTreePlus ID="WorkspaceInspection" __autoremap="1" goal_pose="{workspace}"
robot_name="ur_manipulator" />
<Action ID="EnvManager" env_class="gz_enviroment::GzEnviroment" env_name="gz_enviroment"
service_name="/env_manager/start_env" />
<Action ID="GetWorkspace" type="box" service_name="/get_workspace" workspace="{workspace}"/>
<SubTree ID="WorkspaceMovement" goal_pose="{workspace}" robot_name="rbs_arm" _autoremap="1" />
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<BehaviorTree ID="WorkspaceInspection">
<Sequence>
<Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
pose_vec_out="{goal_pose}"
robot_name="{robot_name}" server_name="/cartesian_move_to_pose" server_timeout="5000" />
<Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
pose_vec_out="{goal_pose}"
robot_name="{robot_name}" server_name="/cartesian_move_to_pose" server_timeout="5000" />
<Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
pose_vec_out="{goal_pose}"
robot_name="{robot_name}" server_name="/cartesian_move_to_pose" server_timeout="5000" />
<Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
pose_vec_out="{goal_pose}"
robot_name="{robot_name}" server_name="/cartesian_move_to_pose" server_timeout="5000" />
<Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
pose_vec_out="{goal_pose}"
robot_name="{robot_name}" server_name="/cartesian_move_to_pose" server_timeout="5000" />
</Sequence>
</BehaviorTree>
<!-- ////////// -->
<include path="./nodes_interfaces/general.xml"/>
<!-- ////////// -->
</root>
</root>

12
rbs_bt_executor/bt_trees/test_tree_env_manager.xml
View file

@ -1,12 +0,0 @@
<?xml version="1.0"?>
<root main_tree_to_execute="EnvManagerBT">
<BehaviorTree ID="EnvManagerBT">
<Sequence>
<Action ID="EnvStarter"
env_name="gz_enviroment"
env_class="gz_enviroment::GzEnviroment"
server_name="/env_manager/start_env"
server_timeout="1000" />
</Sequence>
</BehaviorTree>
</root>

205
rbs_bt_executor/bt_trees/test_tree_main.xml
View file

@ -1,149 +1,72 @@
<?xml version="1.0"?>
<root main_tree_to_execute="MainTree">
<?xml version='1.0' encoding='utf-8'?>
<root main_tree_to_execute="MainTree" BTCPP_format="4">
<BehaviorTree ID="MainTree">
<Sequence>
<Action ID="GetPickPlacePoses"
ObjectName="box1"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
server_name="/get_pick_place_pose_service"
server_timeout="1000"/>
<Action ID="MoveToPose"
robot_name="ur_manipulator_gripper"
pose="{PreGraspPose}"
server_name="/move_topose"
server_timeout="10000"
cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses" ObjectName="box1" GraspDirection="y" PlaceDirection="z" GraspPose="{GraspPose}" PostGraspPose="{PostGraspPose}" PostPostGraspPose="{PostPostGraspPose}" PreGraspPose="{PreGraspPose}" PlacePose="{PlacePose}" PrePlacePose="{PrePlacePose}" PostPlacePose="{PostPlacePose}" server_name="/get_pick_place_pose_service" server_timeout="1000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose="close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose="open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses"
ObjectName="box2"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
server_name="/get_pick_place_pose_service"
server_timeout="1000"/>
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses" ObjectName="box2" GraspDirection="y" PlaceDirection="z" GraspPose="{GraspPose}" PostGraspPose="{PostGraspPose}" PostPostGraspPose="{PostPostGraspPose}" PreGraspPose="{PreGraspPose}" PlacePose="{PlacePose}" PrePlacePose="{PrePlacePose}" PostPlacePose="{PostPlacePose}" server_name="/get_pick_place_pose_service" server_timeout="1000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose="close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose="open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses"
ObjectName="box3"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
server_name="/get_pick_place_pose_service"
server_timeout="1000"/>
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses" ObjectName="box3" GraspDirection="y" PlaceDirection="z" GraspPose="{GraspPose}" PostGraspPose="{PostGraspPose}" PostPostGraspPose="{PostPostGraspPose}" PreGraspPose="{PreGraspPose}" PlacePose="{PlacePose}" PrePlacePose="{PrePlacePose}" PostPlacePose="{PostPlacePose}" server_name="/get_pick_place_pose_service" server_timeout="1000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose="close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose="open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses"
ObjectName="box4"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
server_name="/get_pick_place_pose_service"
server_timeout="1000"/>
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses" ObjectName="box4" GraspDirection="y" PlaceDirection="z" GraspPose="{GraspPose}" PostGraspPose="{PostGraspPose}" PostPostGraspPose="{PostPostGraspPose}" PreGraspPose="{PreGraspPose}" PlacePose="{PlacePose}" PrePlacePose="{PrePlacePose}" PostPlacePose="{PostPlacePose}" server_name="/get_pick_place_pose_service" server_timeout="1000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose="close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose="open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses"
ObjectName="box5"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
server_name="/get_pick_place_pose_service"
server_timeout="1000"/>
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses" ObjectName="box5" GraspDirection="y" PlaceDirection="z" GraspPose="{GraspPose}" PostGraspPose="{PostGraspPose}" PostPostGraspPose="{PostPostGraspPose}" PreGraspPose="{PreGraspPose}" PlacePose="{PlacePose}" PrePlacePose="{PrePlacePose}" PostPlacePose="{PostPlacePose}" server_name="/get_pick_place_pose_service" server_timeout="1000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose="close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose="open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses"
ObjectName="box6"
GraspDirection = "y"
PlaceDirection = "z"
GraspPose="{GraspPose}"
PostGraspPose="{PostGraspPose}"
PostPostGraspPose="{PostPostGraspPose}"
PreGraspPose="{PreGraspPose}"
PlacePose="{PlacePose}"
PrePlacePose="{PrePlacePose}"
PostPlacePose="{PostPlacePose}"
server_name="/get_pick_place_pose_service"
server_timeout="1000"/>
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose = "open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GetPickPlacePoses" ObjectName="box6" GraspDirection="y" PlaceDirection="z" GraspPose="{GraspPose}" PostGraspPose="{PostGraspPose}" PostPostGraspPose="{PostPostGraspPose}" PreGraspPose="{PreGraspPose}" PlacePose="{PlacePose}" PrePlacePose="{PrePlacePose}" PostPlacePose="{PostPlacePose}" server_name="/get_pick_place_pose_service" server_timeout="1000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PreGraspPose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{GraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose="close" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPostGraspPose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PrePlacePose}" server_name="/move_topose" server_timeout="10000" cancel_timeout="5000" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
<Action ID="GripperControl" pose="open" server_name="/gripper_control" server_timeout="10" cancel_timeout="5" />
<Action ID="MoveToPose" robot_name="ur_manipulator_gripper" pose="{PostPlacePose}" server_name="/move_cartesian" server_timeout="10000" cancel_timeout="5000" />
@ -153,8 +76,8 @@
<TreeNodesModel>
<Action ID="MoveToPose">
<input_port name="robot_name"/>
<input_port name="pose_name"/>
<input_port name="robot_name" />
<input_port name="pose_name" />
</Action>
</TreeNodesModel>
</root>

17
rbs_bt_executor/bt_trees/test_tree_pe_cleanup.xml
View file

@ -1,17 +0,0 @@
<?xml version="1.0"?>
<root main_tree_to_execute="PoseEstimation">
<BehaviorTree ID="PoseEstimation">
<Sequence>
<Action ID="PoseEstimation"
ObjectName=""
ReqKind = "deactivate"
server_name="/pose_estimation/change_state"
server_timeout="1000"/>
<Action ID="PoseEstimation"
ObjectName=""
ReqKind = "cleanup"
server_name="/pose_estimation/change_state"
server_timeout="1000"/>
</Sequence>
</BehaviorTree>
</root>

17
rbs_bt_executor/bt_trees/test_tree_pe_configure.xml
View file

@ -1,17 +0,0 @@
<?xml version="1.0"?>
<root main_tree_to_execute="PoseEstimation">
<BehaviorTree ID="PoseEstimation">
<Sequence>
<Action ID="PoseEstimation"
ObjectName="fork"
ReqKind = "configure"
server_name="/pose_estimation/change_state"
server_timeout="1000"/>
<Action ID="PoseEstimation"
ObjectName=""
ReqKind = "activate"
server_name="/pose_estimation/change_state"
server_timeout="1000"/>
</Sequence>
</BehaviorTree>
</root>

17
rbs_bt_executor/bt_trees/workspace_movement.xml Normal file
View file

@ -0,0 +1,17 @@
<?xml version='1.0' encoding='utf-8'?>
<root BTCPP_format="4">
<BehaviorTree ID="WorkspaceMovement">
<Sequence>
<Action ID="MoveToPoseArray" pose_vec_in="{goal_pose}" pose_vec_out="{goal_pose}"
robot_name="{robot_name}" action_name="/move_topose" />
<Action ID="MoveToPoseArray" pose_vec_in="{goal_pose}" pose_vec_out="{goal_pose}"
robot_name="{robot_name}" action_name="/move_cartesian" />
<Action ID="MoveToPoseArray" pose_vec_in="{goal_pose}" pose_vec_out="{goal_pose}"
robot_name="{robot_name}" action_name="/move_cartesian" />
<Action ID="MoveToPoseArray" pose_vec_in="{goal_pose}" pose_vec_out="{goal_pose}"
robot_name="{robot_name}" action_name="/move_cartesian" />
<Action ID="MoveToPoseArray" pose_vec_in="{goal_pose}" pose_vec_out="{goal_pose}"
robot_name="{robot_name}" action_name="/move_cartesian" />
</Sequence>
</BehaviorTree>
</root>

12
rbs_bt_executor/config/bt_executor.yaml Normal file
View file

@ -0,0 +1,12 @@
bt_action_server:
ros__parameters:
action_name: "behavior_server" # Optional (defaults to `bt_action_server`)
tick_frequency: 100 # Optional (defaults to 100 Hz)
groot2_port: 1667 # Optional (defaults to 1667)
ros_plugins_timeout: 1000 # Optional (defaults 1000 ms)
plugins:
- rbs_bt_executor/bt_plugins
behavior_trees:
- rbs_bt_executor/bt_trees

0
rbs_bt_executor/include/rbs_bt_executor/lifecycle_node_bt.hpp Normal file
View file

30
rbs_bt_executor/launch/rbs_executor.launch.py
View file

@ -6,38 +6,18 @@ from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
declared_arguments = []
declared_arguments.append(
DeclareLaunchArgument(
"bt_file",
default_value="test_tree.xml",
description="BehaviorTree file for run.",
)
)
bt_file = LaunchConfiguration("bt_file")
btfile_description = PathJoinSubstitution(
[FindPackageShare("rbs_bt_executor"), "bt_trees", bt_file]
)
btfile_param = {"bt_file_path": btfile_description}
bt_skills_param = PathJoinSubstitution([FindPackageShare("rbs_bt_executor"), "config", "params.yaml"])
executor_params = PathJoinSubstitution([FindPackageShare("rbs_bt_executor"), "config", "bt_executor.yaml"])
nodes_to_start = [
Node(
package='behavior_tree',
executable='bt_engine',
package='rbs_bt_executor',
executable='rbs_bt_executor',
# prefix=['gdbserver localhost:1234'],
parameters=[
btfile_param,
bt_skills_param,
executor_params,
{'use_sim_time': True}
],
# arguments=[
# "--ros-args",
# "--log-level", "debug",
# ],
)
]
return LaunchDescription(declared_arguments + nodes_to_start)
return LaunchDescription(nodes_to_start)

2
rbs_bt_executor/package.xml
View file

@ -11,7 +11,7 @@
<depend>rbs_utils</depend>
<depend>rbs_skill_interfaces</depend>
<depend>env_manager_interfaces</depend>
<depend>behavior_tree</depend>
<depend>behaviortree_ros2</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>

46
rbs_bt_executor/src/AddPlanningSceneObject.cpp
View file

@ -1,46 +0,0 @@
#include "behavior_tree/BtService.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "rbs_skill_interfaces/srv/add_planning_scene_object.hpp"
using namespace BT;
using AddPlanningSceneObjectClient =
rbs_skill_interfaces::srv::AddPlanningSceneObject;
class GetPickPlacePose : public BtService<AddPlanningSceneObjectClient> {
public:
GetPickPlacePose(const std::string &name, const BT::NodeConfiguration &config)
: BtService<AddPlanningSceneObjectClient>(name, config) {
RCLCPP_INFO(_node->get_logger(), "Start the node");
}
AddPlanningSceneObjectClient::Request::SharedPtr populate_request() override {
auto request = std::make_shared<AddPlanningSceneObjectClient::Request>();
RCLCPP_INFO(_node->get_logger(), "Start populate_request()");
object_name_ = getInput<std::string>("ObjectName").value();
auto place_pose_ = getInput<std::vector<double>>("pose").value();
request->object_id = object_name_;
request->object_pose = place_pose_;
return request;
}
BT::NodeStatus handle_response(
AddPlanningSceneObjectClient::Response::SharedPtr response) override {
RCLCPP_INFO(_node->get_logger(), "Response %d", response->success);
return BT::NodeStatus::SUCCESS;
}
static PortsList providedPorts() {
return providedBasicPorts({
InputPort<std::string>("ObjectName"),
InputPort<std::vector<double>>("pose"),
});
}
private:
std::string object_name_{};
};
BT_REGISTER_NODES(factory) {
factory.registerNodeType<GetPickPlacePose>("AddPlanningSceneObject");
}

61
rbs_bt_executor/src/AssembleProcessState.cpp
View file

@ -1,61 +0,0 @@
#include <behavior_tree/BtService.hpp>
#include <rclcpp/rclcpp.hpp>
#include <string>
#include "rbs_skill_interfaces/srv/assemble_state.hpp"
using AssembleStateSrv = rbs_skill_interfaces::srv::AssembleState;
class AssembleState : public BtService<AssembleStateSrv> {
public:
AssembleState(const std::string &name, const BT::NodeConfiguration &config)
: BtService<AssembleStateSrv>(name, config) {
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
}
AssembleStateSrv::Request::SharedPtr populate_request() {
auto request = std::make_shared<AssembleStateSrv::Request>();
auto state_kind = getInput<std::string>("StateKind").value();
request->req_kind = -1;
if (state_kind == "INITIALIZE")
request->req_kind = 0;
else if (state_kind == "VALIDATE")
request->req_kind = 1;
else if (state_kind == "COMPLETE")
request->req_kind = 2;
auto assemble_name = getInput<std::string>("AssembleName").value();
auto part_name = getInput<std::string>("PartName").value();
auto workspace = getInput<std::string>("WorkspaceName").value();
request->assemble_name = assemble_name;
request->part_name = part_name;
request->workspace = workspace;
return request;
}
BT::NodeStatus
handle_response(AssembleStateSrv::Response::SharedPtr response) {
// TODO: return bad status on validate process return false state
return (response->call_status = true) ? BT::NodeStatus::SUCCESS
: BT::NodeStatus::FAILURE;
}
static BT::PortsList providedPorts() {
return providedBasicPorts({BT::InputPort<std::string>("StateKind"),
BT::InputPort<std::string>("AssembleName"),
BT::InputPort<std::string>("PartName"),
BT::InputPort<std::string>("WorkspaceName")});
}
private:
std::map<std::string, int> assemble_states_;
};
#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory) {
factory.registerNodeType<AssembleState>("AssembleState");
}

76
rbs_bt_executor/src/EnvManager.cpp
View file

@ -1,59 +1,43 @@
#include <behavior_tree/BtService.hpp>
#include <behaviortree_cpp_v3/basic_types.h>
#include <env_manager_interfaces/srv/detail/start_env__struct.hpp>
#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
#include <memory>
#include <rclcpp/logging.hpp>
#include <string>
#include "rbs_utils/rbs_utils.hpp"
#include "behaviortree_ros2/bt_service_node.hpp"
#include "env_manager_interfaces/srv/start_env.hpp"
#include "env_manager_interfaces/srv/unload_env.hpp"
#include "geometry_msgs/msg/pose_array.hpp"
using EnvStarterService = env_manager_interfaces::srv::StartEnv;
#include "rbs_utils/rbs_utils.hpp"
#include <behaviortree_cpp/basic_types.h>
#include "behaviortree_ros2/plugins.hpp"
#include <string>
class EnvManagerStarter : public BtService<EnvStarterService> {
using EnvManagerService = env_manager_interfaces::srv::StartEnv;
using namespace BT;
class EnvManager : public RosServiceNode<EnvManagerService> {
public:
EnvManagerStarter(const std::string &name,
const BT::NodeConfiguration &config)
: BtService<EnvStarterService>(name, config) {
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
}
EnvManager(const std::string &name, const NodeConfig &conf,
const RosNodeParams &params)
: RosServiceNode<EnvManagerService>(name, conf, params) {}
EnvStarterService::Request::SharedPtr populate_request() override {
auto request = std::make_shared<EnvStarterService::Request>();
std::string env_name = getInput<std::string>("env_name").value();
std::string env_class = getInput<std::string>("env_class").value();
request->name = env_name;
request->type = env_class;
return request;
}
BT::NodeStatus handle_response(
const EnvStarterService::Response::SharedPtr response) override {
if (response->ok) {
// TODO We need better perfomance for call it in another place for all BT nodes
// - Make it via shared_ptr forward throught blackboard
auto t = std::make_shared<rbs_utils::AssemblyConfigLoader>("asp-example", _node);
auto p = t->getWorkspaceInspectorTrajectory();
setOutput("workspace", p);
return BT::NodeStatus::SUCCESS;
}
return BT::NodeStatus::FAILURE;
}
static BT::PortsList providedPorts() {
static PortsList providedPorts() {
return providedBasicPorts({
BT::InputPort<std::string>("env_name"),
BT::InputPort<std::string>("env_class"),
BT::OutputPort<geometry_msgs::msg::PoseArray>("workspace"),
InputPort<std::string>("env_name"),
InputPort<std::string>("env_class"),
});
}
bool setRequest(Request::SharedPtr &request) override {
getInput("env_name", request->name);
getInput("env_class", request->type);
return true;
}
NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
if (!response->ok) {
return NodeStatus::FAILURE;
}
return NodeStatus::SUCCESS;
}
// virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {}
};
#include "behaviortree_cpp_v3/bt_factory.h"
CreateRosNodePlugin(EnvManager, "EnvManager");
BT_REGISTER_NODES(factory) {
factory.registerNodeType<EnvManagerStarter>("EnvStarter");
}

104
rbs_bt_executor/src/GetPickPlacePoses.cpp
View file

@ -1,104 +0,0 @@
#include "behavior_tree/BtService.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "geometry_msgs/msg/pose.hpp"
#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp"
using namespace BT;
using GetPickPlacePoseClient = rbs_skill_interfaces::srv::GetPickPlacePoses;
class GetPickPlacePose : public BtService<GetPickPlacePoseClient> {
public:
GetPickPlacePose(const std::string &name, const BT::NodeConfiguration &config)
: BtService<GetPickPlacePoseClient>(name, config) {}
GetPickPlacePoseClient::Request::SharedPtr populate_request() override {
auto request = std::make_shared<GetPickPlacePoseClient::Request>();
RCLCPP_INFO(_node->get_logger(), "Start populate_request()");
object_name_ = getInput<std::string>("ObjectName").value();
grasp_direction_str_ = getInput<std::string>("GraspDirection").value();
place_direction_str_ = getInput<std::string>("PlaceDirection").value();
RCLCPP_INFO_STREAM(_node->get_logger(),
"Starting process for object: " << object_name_);
grasp_direction_ = convert_direction_from_string(grasp_direction_str_);
place_direction_ = convert_direction_from_string(place_direction_str_);
request->object_name = object_name_;
request->grasp_direction = grasp_direction_;
request->place_direction = place_direction_;
return request;
}
BT::NodeStatus handle_response(
GetPickPlacePoseClient::Response::SharedPtr response) override {
// RCLCPP_INFO(_node->get_logger(),
// "\n Pose: \n\t pose.x: %f \n\t pose.y: %f \n\t pose.z: %f "
// "\n\n\t opientation.x: %f \n\t orientation.y: %f \n\t "
// "orientation.z: %f \n\t orientation.w: %f",
// response->grasp[1].position.x, response->grasp[1].position.y,
// response->grasp[1].position.z, response->grasp[1].orientation.x,
// response->grasp[1].orientation.y,
// response->grasp[1].orientation.z,
// response->grasp[1].orientation.w);
RCLCPP_INFO(_node->get_logger(), "Start handle_response()");
setOutput<geometry_msgs::msg::Pose>("GraspPose", response->grasp[0]);
setOutput<geometry_msgs::msg::Pose>("PreGraspPose", response->grasp[1]);
setOutput<geometry_msgs::msg::Pose>("PostGraspPose", response->grasp[2]);
setOutput<geometry_msgs::msg::Pose>("PlacePose", response->place[0]);
setOutput<geometry_msgs::msg::Pose>("PrePlacePose", response->place[1]);
setOutput<geometry_msgs::msg::Pose>("PostPlacePose", response->place[2]);
return BT::NodeStatus::SUCCESS;
}
static PortsList providedPorts() {
return providedBasicPorts({
InputPort<std::string>("ObjectName"),
InputPort<std::string>("GraspDirection"), // x or y or z
InputPort<std::string>("PlaceDirection"), // x or y or z
OutputPort<geometry_msgs::msg::Pose>("GraspPose"),
OutputPort<geometry_msgs::msg::Pose>("PreGraspPose"),
OutputPort<geometry_msgs::msg::Pose>("PostGraspPose"),
// TODO: change to std::vector<>
OutputPort<geometry_msgs::msg::Pose>("PostPostGraspPose"),
OutputPort<geometry_msgs::msg::Pose>("PlacePose"),
OutputPort<geometry_msgs::msg::Pose>("PrePlacePose"),
OutputPort<geometry_msgs::msg::Pose>("PostPlacePose"),
});
}
geometry_msgs::msg::Vector3 convert_direction_from_string(std::string str) {
std::map<std::string, int> directions;
geometry_msgs::msg::Vector3 vector;
directions["x"] = 1;
directions["y"] = 2;
directions["z"] = 3;
switch (directions[str]) {
case 1:
vector.x = 1;
vector.y = 0;
vector.z = 0;
break;
case 2:
vector.x = 0;
vector.y = 1;
vector.z = 0;
break;
case 3:
vector.x = 0;
vector.y = 0;
vector.z = 1;
break;
};
return vector;
}
private:
std::string object_name_{};
std::string grasp_direction_str_{};
std::string place_direction_str_{};
geometry_msgs::msg::Vector3 grasp_direction_{};
geometry_msgs::msg::Vector3 place_direction_{};
};
BT_REGISTER_NODES(factory) {
factory.registerNodeType<GetPickPlacePose>("GetPickPlacePoses");
}

65
rbs_bt_executor/src/GetWorkspace.cpp Normal file
View file

@ -0,0 +1,65 @@
#include "behaviortree_ros2/bt_service_node.hpp"
#include "rbs_utils_interfaces/srv/get_workspace.hpp"
#include <behaviortree_cpp/basic_types.h>
#include "behaviortree_ros2/plugins.hpp"
#include <geometry_msgs/msg/detail/point__struct.hpp>
#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
#include <geometry_msgs/msg/detail/quaternion__struct.hpp>
#include <string>
using GetWorkspaceService = rbs_utils_interfaces::srv::GetWorkspace;
using namespace BT;
class GetWorkspace : public RosServiceNode<GetWorkspaceService> {
public:
GetWorkspace(const std::string &name, const NodeConfig &conf,
const RosNodeParams &params)
: RosServiceNode<GetWorkspaceService>(name, conf, params) {}
static PortsList providedPorts() {
return providedBasicPorts({
InputPort<std::string>("type"),
OutputPort<std::shared_ptr<geometry_msgs::msg::PoseArray>>("workspace")
});
}
bool setRequest(Request::SharedPtr &request) override {
getInput("type", request->type);
return true;
}
NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
if (!response->ok) {
return NodeStatus::FAILURE;
}
auto workspace = response->workspace;
auto workspace_arr = std::make_shared<geometry_msgs::msg::PoseArray>();
auto quat = std::make_shared<geometry_msgs::msg::Quaternion>();
quat->w = 0.0;
quat->x = 0.0;
quat->y = 1.0;
quat->z = 0.0;
workspace_arr->poses.resize(workspace.size());
size_t i = 0;
for (auto& point : workspace) {
point.z += 0.35;
geometry_msgs::msg::Pose pose;
pose.position = point;
pose.orientation = *quat;
workspace_arr->poses[i++] = pose;
}
setOutput("workspace", workspace_arr);
return NodeStatus::SUCCESS;
}
// virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {}
};
CreateRosNodePlugin(GetWorkspace, "GetWorkspace");

51
rbs_bt_executor/src/MoveGripper.cpp Normal file
View file

@ -0,0 +1,51 @@
#include "behaviortree_ros2/bt_action_node.hpp"
#include "rbs_skill_interfaces/action/gripper_command.hpp"
#include <behaviortree_cpp/tree_node.h>
#include <behaviortree_ros2/plugins.hpp>
#include <behaviortree_ros2/ros_node_params.hpp>
using GripperCommand = rbs_skill_interfaces::action::GripperCommand;
using namespace BT;
class GripperControl : public RosActionNode<GripperCommand> {
public:
GripperControl(const std::string &name, const NodeConfig &conf,
const RosNodeParams &params)
: RosActionNode<GripperCommand>(name, conf, params) {
if (!node_.lock()->has_parameter("open")) {
node_.lock()->declare_parameter("open", position.open);
}
if (!node_.lock()->has_parameter("close")) {
node_.lock()->declare_parameter("close", position.close);
}
}
static PortsList providedPorts() {
return providedBasicPorts({
InputPort<std::string>("pose"),
});
}
bool setGoal(RosActionNode::Goal &goal) override {
getInput("pose", pose);
goal.position = node_.lock()->get_parameter(pose).as_double();
return true;
}
NodeStatus onResultReceived(const WrappedResult& wr) override {
if (!wr.result->success) {
return NodeStatus::FAILURE;
}
return NodeStatus::SUCCESS;
}
private:
struct {
double open = 0.008;
double close = 0.000;
} position;
std::string pose;
};
CreateRosNodePlugin(GripperControl, "MoveGripper");

70
rbs_bt_executor/src/MoveToJointStates.cpp
View file

@ -1,56 +1,40 @@
#include "rbs_skill_interfaces/action/moveit_send_joint_states.hpp"
#include "behavior_tree/BtAction.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "behaviortree_ros2/bt_action_node.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "moveit_msgs/action/move_group.h"
// #include "Eigen/Geometry"
#include "rclcpp/parameter.hpp"
#include <behaviortree_cpp/basic_types.h>
#include <behaviortree_cpp/tree_node.h>
#include <behaviortree_ros2/plugins.hpp>
#include <behaviortree_ros2/ros_node_params.hpp>
#include "rbs_skill_interfaces/action/moveit_send_joint_states.hpp"
using namespace BT;
using MoveToJointStateAction =
rbs_skill_interfaces::action::MoveitSendJointStates;
static const rclcpp::Logger LOGGER =
rclcpp::get_logger("MoveToJointStateActionClient");
class MoveToJointState : public BtAction<MoveToJointStateAction> {
class MoveToJointState : public RosActionNode<MoveToJointStateAction> {
public:
MoveToJointState(const std::string &name, const BT::NodeConfiguration &config)
: BtAction<MoveToJointStateAction>(name, config) {}
MoveToJointStateAction::Goal populate_goal() override {
getInput<std::string>("robot_name", robot_name_);
getInput<std::vector<double>>("JointState", joint_values_);
auto goal = MoveToJointStateAction::Goal();
RCLCPP_INFO(_node->get_logger(), "Send goal for robot [%s]",
robot_name_.c_str());
for (auto joint : joint_values_) {
RCLCPP_INFO_STREAM(_node->get_logger(), "Joint value " << joint);
}
RCLCPP_INFO_STREAM(_node->get_logger(),
"Joint value size " << joint_values_.size());
goal.robot_name = robot_name_;
goal.joint_values = joint_values_;
RCLCPP_INFO(_node->get_logger(), "Goal populated");
return goal;
}
MoveToJointState(const std::string &name, const NodeConfig &conf,
const RosNodeParams &params)
: RosActionNode<MoveToJointStateAction>(name, conf, params) {}
static PortsList providedPorts() {
return providedBasicPorts(
{InputPort<std::string>("robot_name"),
InputPort<std::vector<double>>("JointState")});
return providedBasicPorts({InputPort<std::string>("robot_name"),
InputPort<std::vector<double>>("JointState")});
}
private:
std::string robot_name_{};
std::vector<double> joint_values_;
}; // class MoveToJointState
bool setGoal(RosActionNode::Goal& goal) override {
getInput("robot_name", goal.robot_name);
getInput("JointState", goal.joint_values);
return true;
}
BT_REGISTER_NODES(factory) {
factory.registerNodeType<MoveToJointState>("MoveToJointState");
}
NodeStatus onResultReceived(const WrappedResult& wr) override {
if (!wr.result->success) {
return NodeStatus::FAILURE;
}
return NodeStatus::SUCCESS;
}
};
CreateRosNodePlugin(MoveToJointState, "MoveToJointState");

70
rbs_bt_executor/src/MoveToPose.cpp
View file

@ -1,53 +1,37 @@
// Copyright [2023] bill-finger
#include "behaviortree_ros2/bt_action_node.hpp"
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
#include <behaviortree_cpp/tree_node.h>
#include <behaviortree_ros2/plugins.hpp>
#include <behaviortree_ros2/ros_node_params.hpp>
#include "behavior_tree/BtAction.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
// #include "Eigen/Geometry"
using namespace BT;
using MoveitSendPoseAction = rbs_skill_interfaces::action::MoveitSendPose;
using MoveToPoseAction = rbs_skill_interfaces::action::MoveitSendPose;
class MoveToPose : public BtAction<MoveToPoseAction> {
public:
MoveToPose(const std::string &name, const BT::NodeConfiguration &config)
: BtAction<MoveToPoseAction>(name, config) {}
MoveToPoseAction::Goal populate_goal() override {
getInput<std::string>("robot_name", robot_name_);
getInput<geometry_msgs::msg::Pose>("pose", pose_des);
RCLCPP_INFO(_node->get_logger(),
"\n Send Pose: \n\t pose.x: %f \n\t pose.y: %f \n\t pose.z: %f "
"\n\n\t opientation.x: %f \n\t orientation.y: %f \n\t "
"orientation.z: %f \n\t orientation.w: %f",
pose_des.position.x, pose_des.position.y, pose_des.position.z,
pose_des.orientation.x, pose_des.orientation.y,
pose_des.orientation.z, pose_des.orientation.w);
auto goal = MoveToPoseAction::Goal();
RCLCPP_INFO(_node->get_logger(), "Send goal to robot [%s]",
robot_name_.c_str());
goal.robot_name = robot_name_;
goal.target_pose = pose_des;
goal.end_effector_acceleration = 1.0;
goal.end_effector_velocity = 1.0;
RCLCPP_INFO(_node->get_logger(), "Goal populated");
return goal;
}
class MoveToPose : public RosActionNode<MoveitSendPoseAction> {
public:
MoveToPose(const std::string &name, const NodeConfig &conf,
const RosNodeParams &params)
: RosActionNode<MoveitSendPoseAction>(name, conf, params) {}
static BT::PortsList providedPorts() {
return providedBasicPorts(
{BT::InputPort<std::string>("robot_name"),
BT::InputPort<geometry_msgs::msg::Pose>("pose")});
BT::InputPort<std::shared_ptr<geometry_msgs::msg::Pose>>("pose")});
}
private:
std::string robot_name_;
geometry_msgs::msg::Pose pose_des;
}; // class MoveToPose
bool setGoal(RosActionNode::Goal& goal) override {
getInput("robot_name", goal.robot_name);
getInput("pose", goal.target_pose);
return true;
}
BT_REGISTER_NODES(factory) {
factory.registerNodeType<MoveToPose>("MoveToPose");
}
NodeStatus onResultReceived(const WrappedResult& wr) override {
if (!wr.result->success) {
return NodeStatus::FAILURE;
}
return NodeStatus::SUCCESS;
}
};
CreateRosNodePlugin(MoveToPose, "MoveToPose");

85
rbs_bt_executor/src/MoveToPoseArray.cpp
View file

@ -1,58 +1,55 @@
#include "behavior_tree/BtAction.hpp"
#include "behaviortree_cpp_v3/behavior_tree.h"
#include "behaviortree_ros2/bt_action_node.hpp"
#include "geometry_msgs/msg/pose_array.hpp"
#include "rbs_skill_interfaces/action/moveit_send_pose.hpp"
#include "rbs_utils/rbs_utils.hpp"
#include <behaviortree_cpp_v3/bt_factory.h>
#include <behaviortree_cpp/tree_node.h>
#include <behaviortree_ros2/plugins.hpp>
#include <behaviortree_ros2/ros_node_params.hpp>
#include <geometry_msgs/msg/detail/pose_array__struct.hpp>
#include <rclcpp/logging.hpp>
#include <rclcpp/node.hpp>
using namespace BT;
using MoveToPoseArrayAction = rbs_skill_interfaces::action::MoveitSendPose;
class MoveToPoseArray : public BtAction<MoveToPoseArrayAction> {
class MoveToPoseArray : public RosActionNode<MoveToPoseArrayAction> {
public:
MoveToPoseArray(const std::string &name, const BT::NodeConfiguration &config)
: BtAction<MoveToPoseArrayAction>(name, config) {
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
}
MoveToPoseArray(const std::string &name, const NodeConfig &conf,
const RosNodeParams &params)
: RosActionNode<MoveToPoseArrayAction>(name, conf, params) {}
MoveToPoseArrayAction::Goal populate_goal() override {
auto goal = MoveToPoseArrayAction::Goal();
getInput<std::string>("robot_name", robot_name_);
getInput<geometry_msgs::msg::PoseArray>("pose_vec_in", target_pose_vec_);
for (auto &point : target_pose_vec_.poses) {
RCLCPP_INFO(_node->get_logger(), "Pose array: \n %f \n %f \n %f",
point.position.x, point.position.y, point.position.z);
}
if (!target_pose_vec_.poses.empty()) {
goal.robot_name = robot_name_;
goal.target_pose = target_pose_vec_.poses.at(0);
goal.end_effector_acceleration = 1.0;
goal.end_effector_velocity = 1.0;
target_pose_vec_.poses.erase(target_pose_vec_.poses.begin());
setOutput<geometry_msgs::msg::PoseArray>("pose_vec_out",
target_pose_vec_);
} else {
RCLCPP_WARN(_node->get_logger(), "Target pose vector empty");
}
return goal;
}
static BT::PortsList providedPorts() {
static PortsList providedPorts() {
return providedBasicPorts(
{BT::InputPort<std::string>("robot_name"),
BT::InputPort<geometry_msgs::msg::PoseArray>("pose_vec_in"),
BT::OutputPort<geometry_msgs::msg::PoseArray>("pose_vec_out")});
{InputPort<std::string>("robot_name"),
InputPort<std::shared_ptr<geometry_msgs::msg::PoseArray>>(
"pose_vec_in"),
OutputPort<std::shared_ptr<geometry_msgs::msg::PoseArray>>(
"pose_vec_out")});
}
bool setGoal(RosActionNode::Goal &goal) override {
getInput("robot_name", goal.robot_name);
getInput("pose_vec_in", m_pose_vec);
if (m_pose_vec->poses.empty()) {
return false;
}
goal.target_pose = m_pose_vec->poses[0];
goal.end_effector_velocity = 1.0;
goal.end_effector_acceleration = 1.0;
m_pose_vec->poses.erase(m_pose_vec->poses.begin());
setOutput("pose_vec_out", m_pose_vec);
return true;
}
NodeStatus onResultReceived(const WrappedResult &wr) override {
if (!wr.result->success) {
return NodeStatus::FAILURE;
}
return NodeStatus::SUCCESS;
}
private:
std::string robot_name_;
geometry_msgs::msg::PoseArray target_pose_vec_;
std::shared_ptr<geometry_msgs::msg::PoseArray> m_pose_vec;
};
BT_REGISTER_NODES(factory) {
factory.registerNodeType<MoveToPoseArray>("MoveToPoseArray");
}
CreateRosNodePlugin(MoveToPoseArray, "MoveToPoseArray");

125
rbs_bt_executor/src/ObjectDetection.cpp
View file

@ -1,69 +1,64 @@
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <behavior_tree/BtService.hpp>
#include <behaviortree_cpp_v3/basic_types.h>
#include <rclcpp/logging.hpp>
#include <string>
#include "rcl_interfaces/msg/parameter.hpp"
#include "rcl_interfaces/srv/set_parameters.hpp"
#include "behaviortree_ros2/bt_service_node.hpp"
#include "lifecycle_msgs/msg/transition.hpp"
#include "lifecycle_msgs/srv/change_state.hpp"
#include "rcl_interfaces/msg/parameter.hpp"
#include "rcl_interfaces/srv/set_parameters.hpp"
#include <behaviortree_cpp/tree_node.h>
#include <behaviortree_ros2/plugins.hpp>
#include <behaviortree_ros2/ros_node_params.hpp>
#include <memory>
#include <rclcpp/parameter_client.hpp>
using namespace BT;
using namespace std::chrono_literals;
using ObjDetectionSrv = lifecycle_msgs::srv::ChangeState;
class ObjectDetection : public BtService<ObjDetectionSrv> {
class ObjectDetection : public RosServiceNode<ObjDetectionSrv> {
public:
ObjectDetection(const std::string &name, const BT::NodeConfiguration &config)
: BtService<ObjDetectionSrv>(name, config) {
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
_set_params_client = std::make_shared<rclcpp::AsyncParametersClient>(
_node, "/object_detection");
while (!_set_params_client->wait_for_service(1s)) {
ObjectDetection(const std::string &name, const NodeConfig &conf,
const RosNodeParams &params)
: RosServiceNode<ObjDetectionSrv>(name, conf, params) {
m_params_client = std::make_shared<rclcpp::AsyncParametersClient>(
node_.lock(), "/object_detection");
while (!m_params_client->wait_for_service(1s)) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(_node->get_logger(),
RCLCPP_ERROR(logger(),
"Interrupted while waiting for the service. Exiting.");
break;
}
RCLCPP_WARN(_node->get_logger(),
"service not available, waiting again...");
}
_setting_path = getInput<std::string>("SettingPath").value();
getInput("SettingPath", m_settings_path);
}
ObjDetectionSrv::Request::SharedPtr populate_request() {
auto request = std::make_shared<ObjDetectionSrv::Request>();
_req_type = getInput<std::string>("ReqKind").value();
request->set__transition(transition_event(_req_type));
return request;
}
BT::NodeStatus
handle_response(const ObjDetectionSrv::Response::SharedPtr response) {
if (response->success) {
return BT::NodeStatus::SUCCESS;
}
return BT::NodeStatus::FAILURE;
}
static BT::PortsList providedPorts() {
static PortsList providedPorts() {
return providedBasicPorts({
BT::InputPort<std::string>("ReqKind"),
BT::InputPort<std::string>("SettingPath"),
InputPort<std::string>("ReqKind"),
InputPort<std::string>("SettingPath"),
});
}
bool setRequest(Request::SharedPtr &request) override {
getInput("ReqKind", m_req_type);
auto transition = transition_event(m_req_type);
request->set__transition(transition);
return true;
}
NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
if (!response->success) {
return NodeStatus::FAILURE;
}
return NodeStatus::SUCCESS;
}
private:
uint8_t transition_id_{};
std::string _setting_path{};
// std::string _model_path{};
std::string _req_type{};
std::shared_ptr<rclcpp::AsyncParametersClient> _set_params_client;
std::string m_settings_path{};
// std::string _model_path{};
std::string m_req_type{};
std::shared_ptr<rclcpp::AsyncParametersClient> m_params_client;
std::vector<rcl_interfaces::msg::Parameter> _params;
rcl_interfaces::msg::Parameter _param;
rcl_interfaces::msg::Parameter m_param;
lifecycle_msgs::msg::Transition
transition_event(const std::string &req_type) {
@ -82,38 +77,14 @@ private:
return translation;
}
// inline std::string build_model_path(const std::string &model_name,
// const std::string &package_path) {
// return package_path + "/config/" + model_name + ".ply";
// }
// inline std::string build_model_path(const std::string &model_path) {
// return model_path;
// }
void set_str_param() {
RCLCPP_INFO_STREAM(_node->get_logger(),
"Set string parameter: <" + _setting_path + ">");
RCLCPP_INFO_STREAM(logger(),
"Set string parameter: <" + m_settings_path + ">");
std::vector<rclcpp::Parameter> _parameters{
rclcpp::Parameter("setting_path", _setting_path)};
_set_params_client->set_parameters(_parameters);
std::vector<rclcpp::Parameter> t_parameters{
rclcpp::Parameter("setting_path", m_settings_path)};
m_params_client->set_parameters(t_parameters);
}
// void set_all_param() {
// auto _package_name =
// ament_index_cpp::get_package_share_directory("rbs_perception");
// _model_path = build_model_path(_setting_path, _package_name);
// RCLCPP_INFO_STREAM(_node->get_logger(), _model_path);
// std::vector<rclcpp::Parameter> _parameters{
// rclcpp::Parameter("setting_path", _setting_path)};
// _set_params_client->set_parameters(_parameters);
// }
};
#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory) {
factory.registerNodeType<ObjectDetection>("ObjectDetection");
}
CreateRosNodePlugin(ObjectDetection, "ObjectDetection");

121
rbs_bt_executor/src/PoseEstimation.cpp
View file

@ -1,89 +1,94 @@
#include <ament_index_cpp/get_package_share_directory.hpp>
#include <behavior_tree/BtService.hpp>
#include <behaviortree_cpp_v3/basic_types.h>
#include <rclcpp/logging.hpp>
#include <string>
#include "behaviortree_ros2/bt_service_node.hpp"
#include "rcl_interfaces/msg/parameter.hpp"
#include "rcl_interfaces/srv/set_parameters.hpp"
#include "ament_index_cpp/get_package_share_directory.hpp"
#include "lifecycle_msgs/msg/transition.hpp"
#include "lifecycle_msgs/srv/change_state.hpp"
#include <behaviortree_ros2/plugins.hpp>
#include <memory>
using namespace BT;
using namespace std::chrono_literals;
using PoseEstimationSrv = lifecycle_msgs::srv::ChangeState;
class PoseEstimation : public BtService<PoseEstimationSrv> {
class PoseEstimation : public RosServiceNode<PoseEstimationSrv> {
public:
PoseEstimation(const std::string &name, const BT::NodeConfiguration &config)
: BtService<PoseEstimationSrv>(name, config) {
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
PoseEstimation(const std::string &name, const NodeConfig &conf,
const RosNodeParams &params)
: RosServiceNode<PoseEstimationSrv>(name, conf, params) {
RCLCPP_INFO_STREAM(logger(), "Start node.");
_set_params_client = std::make_shared<rclcpp::AsyncParametersClient>(
_node, "/pose_estimation");
m_params_client = std::make_shared<rclcpp::AsyncParametersClient>(
node_.lock(), "/pose_estimation");
while (!_set_params_client->wait_for_service(1s)) {
while (!m_params_client->wait_for_service(1s)) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(_node->get_logger(),
RCLCPP_ERROR(logger(),
"Interrupted while waiting for the service. Exiting.");
break;
}
RCLCPP_WARN(_node->get_logger(),
"service not available, waiting again...");
RCLCPP_WARN(logger(), "service not available, waiting again...");
}
_model_name = getInput<std::string>("ObjectName").value();
// Get model name paramter from BT ports
getInput("ObjectName", m_model_name);
}
PoseEstimationSrv::Request::SharedPtr populate_request() {
auto request = std::make_shared<PoseEstimationSrv::Request>();
_req_type = getInput<std::string>("ReqKind").value();
request->set__transition(transition_event(_req_type));
return request;
bool setRequest(Request::SharedPtr &request) override {
getInput("ReqKind", m_req_type);
request->set__transition(transition_event(m_req_type));
return true;
}
BT::NodeStatus
handle_response(const PoseEstimationSrv::Response::SharedPtr response) {
if (response->success) {
return BT::NodeStatus::SUCCESS;
NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
if (!response->success) {
return NodeStatus::FAILURE;
}
return BT::NodeStatus::FAILURE;
return NodeStatus::SUCCESS;
}
static BT::PortsList providedPorts() {
static PortsList providedPorts() {
return providedBasicPorts({
BT::InputPort<std::string>("ReqKind"),
BT::InputPort<std::string>("ObjectName"),
BT::InputPort<std::string>("ObjectPath"),
InputPort<std::string>("ReqKind"),
InputPort<std::string>("ObjectName"),
InputPort<std::string>("ObjectPath"),
});
}
private:
uint8_t transition_id_{};
std::string _model_name{};
std::string _model_path{};
std::string _req_type{};
std::shared_ptr<rclcpp::AsyncParametersClient> _set_params_client;
std::vector<rcl_interfaces::msg::Parameter> _params;
rcl_interfaces::msg::Parameter _param;
uint8_t m_transition_id{};
std::string m_model_name{};
std::string m_model_type{};
std::string m_req_type{};
std::shared_ptr<rclcpp::AsyncParametersClient> m_params_client;
std::vector<rcl_interfaces::msg::Parameter> m_params;
rcl_interfaces::msg::Parameter m_param;
lifecycle_msgs::msg::Transition
transition_event(const std::string &req_type) {
lifecycle_msgs::msg::Transition translation{};
// ParamSetter param_setter(m_params_client);
if (req_type == "configure") {
set_mesh_param();
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE;
// auto str_mesh_param = std::make_shared<SetParamShareDirectoryStrategy>("model_name", m_model_name);
// param_setter.setStrategy(str_mesh_param);
// param_setter.setParam()
m_transition_id = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE;
} else if (req_type == "calibrate") {
set_str_param();
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE;
m_transition_id = lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE;
} else if (req_type == "activate") {
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE;
m_transition_id = lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE;
} else if (req_type == "deactivate") {
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE;
m_transition_id = lifecycle_msgs::msg::Transition::TRANSITION_DEACTIVATE;
} else if (req_type == "cleanup") {
transition_id_ = lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP;
m_transition_id = lifecycle_msgs::msg::Transition::TRANSITION_CLEANUP;
}
// calibrate
translation.set__id(transition_id_);
translation.set__id(m_transition_id);
return translation;
}
@ -97,28 +102,24 @@ private:
}
void set_str_param() {
RCLCPP_INFO_STREAM(_node->get_logger(),
"Set string parameter: <" + _model_name + ">");
RCLCPP_INFO_STREAM(logger(),
"Set string parameter: <" + m_model_name + ">");
std::vector<rclcpp::Parameter> _parameters{
rclcpp::Parameter("mesh_path", _model_name)};
_set_params_client->set_parameters(_parameters);
std::vector<rclcpp::Parameter> params{
rclcpp::Parameter("model_name", m_model_name)};
m_params_client->set_parameters(params);
}
void set_mesh_param() {
auto _package_name =
auto t_package_name =
ament_index_cpp::get_package_share_directory("rbs_perception");
_model_path = build_model_path(_model_name, _package_name);
RCLCPP_INFO_STREAM(_node->get_logger(), _model_path);
m_model_type = build_model_path(m_model_name, t_package_name);
RCLCPP_INFO_STREAM(logger(), m_model_type);
std::vector<rclcpp::Parameter> _parameters{
rclcpp::Parameter("mesh_path", _model_name)};
_set_params_client->set_parameters(_parameters);
std::vector<rclcpp::Parameter> params{
rclcpp::Parameter("mesh_path", m_model_name)};
m_params_client->set_parameters(params);
}
};
#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory) {
factory.registerNodeType<PoseEstimation>("PoseEstimation");
}
CreateRosNodePlugin(PoseEstimation, "PoseEstimation");

43
rbs_bt_executor/src/TreeRunner.cpp Normal file
View file

@ -0,0 +1,43 @@
#include <behaviortree_ros2/tree_execution_server.hpp>
#include <behaviortree_cpp/loggers/bt_cout_logger.h>
// #include <rclcpp/logging.hpp>
// #include <std_msgs/msg/float32.hpp>
class RbsBtExecutor : public BT::TreeExecutionServer {
public:
RbsBtExecutor(const rclcpp::NodeOptions &options)
: TreeExecutionServer(options) {}
void onTreeCreated(BT::Tree &tree) override {
logger_cout_ = std::make_shared<BT::StdCoutLogger>(tree);
}
std::optional<std::string>
onTreeExecutionCompleted(BT::NodeStatus status, bool was_cancelled) override {
// RCLCPP_INFO(logger_cout_, "Tree completed with code: %d", status );
logger_cout_.reset();
return std::nullopt;
}
private:
std::shared_ptr<BT::StdCoutLogger> logger_cout_;
// rclcpp::Subscription<std_msgs::msg::Float32>::SharedPtr sub_;
};
int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
rclcpp::NodeOptions options;
auto action_server = std::make_shared<RbsBtExecutor>(options);
rclcpp::executors::MultiThreadedExecutor exec(
rclcpp::ExecutorOptions(), 0, false, std::chrono::milliseconds(250));
exec.add_node(action_server->node());
exec.spin();
exec.remove_node(action_server->node());
rclcpp::shutdown();
}

53
rbs_bt_executor/src/gripper_move.cpp
View file

@ -1,53 +0,0 @@
#include "rbs_skill_interfaces/action/gripper_command.hpp"
#include "behavior_tree/BtAction.hpp"
#include "behaviortree_cpp_v3/bt_factory.h"
#include "rclcpp/parameter.hpp"
using namespace BT;
using GripperCommand = rbs_skill_interfaces::action::GripperCommand;
class GripperControl : public BtAction<GripperCommand> {
public:
GripperControl(const std::string &name, const BT::NodeConfiguration &config)
: BtAction<GripperCommand>(name, config) {
if (!_node->has_parameter("open")) {
_node->declare_parameter("open", position.open);
}
if (!_node->has_parameter("close")) {
_node->declare_parameter("close", position.close);
}
}
GripperCommand::Goal populate_goal() override {
getInput<std::string>("pose", pose);
double targetPose = _node->get_parameter(pose).as_double();
RCLCPP_INFO(_node->get_logger(), "Send target pose: %f", targetPose);
auto goal = GripperCommand::Goal();
goal.position = targetPose;
RCLCPP_INFO(_node->get_logger(), "Goal send");
return goal;
}
static PortsList providedPorts() {
return providedBasicPorts({InputPort<std::string>("pose")});
}
private:
struct {
double open = 0.008;
double close = 0.000;
} position;
std::string pose;
};
BT_REGISTER_NODES(factory) {
factory.registerNodeType<GripperControl>("GripperControl");
}

64
rbs_bt_executor/src/rbsBTAction.cpp
View file

@ -1,62 +1,62 @@
#include "behavior_tree/BtService.hpp"
// #include "behavior_tree/BtService.hpp"
#include "behaviortree_ros2/bt_service_node.hpp"
#include "rbs_skill_interfaces/srv/rbs_bt.hpp"
#include <behaviortree_cpp/tree_node.h>
#include <behaviortree_ros2/plugins.hpp>
#include <behaviortree_ros2/ros_node_params.hpp>
#define STR_ACTION "do"
#define STR_SID "sid"
#define STR_COMMAND "command"
#define NODE_NAME "rbs_interface"
using namespace BT;
using namespace std::chrono_literals;
using RbsBtActionSrv = rbs_skill_interfaces::srv::RbsBt;
class RbsBtAction : public BtService<RbsBtActionSrv> {
class RbsBtAction : public RosServiceNode<RbsBtActionSrv> {
public:
RbsBtAction(const std::string &name, const BT::NodeConfiguration &config)
: BtService<RbsBtActionSrv>(name, config) {
RbsBtAction(const std::string &name, const NodeConfig& conf, const RosNodeParams& params)
: RosServiceNode<RbsBtActionSrv>(name, conf, params) {
_action_name = getInput<std::string>(STR_ACTION).value();
RCLCPP_INFO_STREAM(_node->get_logger(), "Start node RbsBtAction: " + _action_name);
m_action_name = getInput<std::string>(STR_ACTION).value();
RCLCPP_INFO_STREAM(logger(), "Start node RbsBtAction: " + m_action_name);
_set_params_client = std::make_shared<rclcpp::AsyncParametersClient>(_node, NODE_NAME);
m_params_client = std::make_shared<rclcpp::AsyncParametersClient>(node_.lock(), NODE_NAME);
while (!_set_params_client->wait_for_service(1s)) {
while (!m_params_client->wait_for_service(1s)) {
if (!rclcpp::ok()) {
RCLCPP_ERROR(_node->get_logger(), "Interrupted while waiting for the service. Exiting.");
RCLCPP_ERROR(logger(), "Interrupted while waiting for the service. Exiting.");
break;
}
RCLCPP_WARN(_node->get_logger(), NODE_NAME " service not available, waiting again...");
RCLCPP_WARN(logger(), NODE_NAME " service not available, waiting again...");
}
}
RbsBtActionSrv::Request::SharedPtr populate_request() override {
auto request = std::make_shared<RbsBtActionSrv::Request>();
request->action = _action_name;
request->sid = getInput<std::string>(STR_SID).value();
request->command = getInput<std::string>(STR_COMMAND).value();
return request;
bool setRequest(Request::SharedPtr &request) override {
getInput(STR_SID, request->sid);
getInput(STR_COMMAND, request->command);
return true;
}
BT::NodeStatus handle_response(const RbsBtActionSrv::Response::SharedPtr response) override {
if (response->ok) {
return BT::NodeStatus::SUCCESS;
NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
if (!response->ok) {
return NodeStatus::FAILURE;
}
return BT::NodeStatus::FAILURE;
return NodeStatus::SUCCESS;
}
static BT::PortsList providedPorts() {
static PortsList providedPorts() {
return providedBasicPorts({
BT::InputPort<std::string>(STR_SID),
BT::InputPort<std::string>(STR_ACTION),
BT::InputPort<std::string>(STR_COMMAND)
InputPort<std::string>(STR_SID),
InputPort<std::string>(STR_ACTION),
InputPort<std::string>(STR_COMMAND)
});
}
private:
std::string _action_name{};
std::shared_ptr<rclcpp::AsyncParametersClient> _set_params_client;
std::string m_action_name{};
std::shared_ptr<rclcpp::AsyncParametersClient> m_params_client;
};
#include "behaviortree_cpp_v3/bt_factory.h"
BT_REGISTER_NODES(factory) {
factory.registerNodeType<RbsBtAction>("RbsBtAction");
}
CreateRosNodePlugin(RbsBtAction, "RbsAction")

2
rbs_simulation/launch/simulation_gazebo.launch.py
View file

@ -28,7 +28,7 @@ def generate_launch_description():
)
declared_arguments.append(
DeclareLaunchArgument("launch_env_manager",
default_value="false",
default_value="true",
description="Launch env_manager?")
)
declared_arguments.append(

35
rbs_skill_servers/CMakeLists.txt
View file

@ -110,18 +110,18 @@ rclcpp_components_register_node(
EXECUTABLE gripper_control_action_server)
# -- PickPlacePoseLoader --
add_library(pick_place_pose_loader SHARED src/pick_place_pose_loader.cpp)
target_include_directories(
pick_place_pose_loader
PRIVATE $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
target_compile_definitions(pick_place_pose_loader
PRIVATE "PICK_PLACE_POSE_LOADER_CPP_BUILDING_DLL")
ament_target_dependencies(pick_place_pose_loader ${deps} Eigen3
rbs_utils)
rclcpp_components_register_node(
pick_place_pose_loader PLUGIN "rbs_skill_actions::GetGraspPickPoseServer"
EXECUTABLE pick_place_pose_loader_service_server)
# add_library(pick_place_pose_loader SHARED src/pick_place_pose_loader.cpp)
# target_include_directories(
# pick_place_pose_loader
# PRIVATE $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
# $<INSTALL_INTERFACE:include>)
# target_compile_definitions(pick_place_pose_loader
# PRIVATE "PICK_PLACE_POSE_LOADER_CPP_BUILDING_DLL")
# ament_target_dependencies(pick_place_pose_loader ${deps} Eigen3
# rbs_utils)
# rclcpp_components_register_node(
# pick_place_pose_loader PLUGIN "rbs_skill_actions::GetGraspPickPoseServer"
# EXECUTABLE pick_place_pose_loader_service_server)
# -- MoveitActionServers --
add_executable(move_to_joint_states_action_server
@ -136,10 +136,10 @@ add_executable(move_cartesian_path_action_server
ament_target_dependencies(move_cartesian_path_action_server ${deps})
add_executable(servo_action_server
src/moveit_servo_skill_server.cpp)
ament_target_dependencies(servo_action_server ${deps})
# add_executable(servo_action_server
# src/moveit_servo_skill_server.cpp)
# ament_target_dependencies(servo_action_server ${deps})
#
install(DIRECTORY include/ DESTINATION include)
install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})
@ -147,10 +147,9 @@ install(DIRECTORY launch config DESTINATION share/${PROJECT_NAME})
install(
TARGETS move_topose_action_server
gripper_action_server
pick_place_pose_loader
move_to_joint_states_action_server
move_cartesian_path_action_server
servo_action_server
# servo_action_server
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION lib/${PROJECT_NAME})

72
rbs_skill_servers/src/add_planning_scene_objects_service.cpp
View file

@ -1,72 +0,0 @@
#include "moveit/move_group_interface/move_group_interface.h"
#include "rbs_skill_interfaces/srv/add_planning_scene_object.hpp"
#include <algorithm>
#include <cinttypes>
#include <memory>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include "rclcpp/rclcpp.hpp"
using AddPlanningSceneObject =
rbs_skill_interfaces::srv::AddPlanningSceneObject;
rclcpp::Node::SharedPtr g_node = nullptr;
void handle_service(
const std::shared_ptr<rmw_request_id_t> request_header,
const std::shared_ptr<AddPlanningSceneObject::Request> request,
const std::shared_ptr<AddPlanningSceneObject::Response> response) {
(void)request_header;
// Create collision object for the robot to avoid
auto const collision_object = [frame_id = "world",
object_name = request->object_id,
object_pose = request->object_pose] {
moveit_msgs::msg::CollisionObject collision_object;
collision_object.header.frame_id = frame_id;
collision_object.id = object_name;
shape_msgs::msg::SolidPrimitive primitive;
// Define the size of the box in meters
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[primitive.BOX_X] = 0.05;
primitive.dimensions[primitive.BOX_Y] = 0.05;
primitive.dimensions[primitive.BOX_Z] = 0.05;
// Define the pose of the box (relative to the frame_id)
geometry_msgs::msg::Pose box_pose;
box_pose.position.x = object_pose[0];
box_pose.position.y = object_pose[1];
box_pose.position.z = object_pose[2];
box_pose.orientation.x = object_pose[3];
box_pose.orientation.y = object_pose[4];
box_pose.orientation.z = object_pose[5];
box_pose.orientation.w = object_pose[6];
collision_object.primitives.push_back(primitive);
collision_object.primitive_poses.push_back(box_pose);
collision_object.operation = collision_object.ADD;
return collision_object;
}();
// Add the collision object to the scene
moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
planning_scene_interface.applyCollisionObject(collision_object);
response->success = true;
}
int main(int argc, char **argv) {
rclcpp::init(argc, argv);
rclcpp::NodeOptions node_options;
node_options.allow_undeclared_parameters(true)
.automatically_declare_parameters_from_overrides(true);
g_node =
rclcpp::Node::make_shared("add_planing_scene_object", "", node_options);
auto server = g_node->create_service<AddPlanningSceneObject>(
"add_planing_scene_object_service", handle_service);
rclcpp::spin(g_node);
rclcpp::shutdown();
g_node = nullptr;
return 0;
}

245
rbs_skill_servers/src/assemble_state_server.cpp
View file

@ -1,245 +0,0 @@
#include <filesystem>
#include <iostream>
#include <memory>
#include <string>
#include <vector>
#include <rclcpp/rclcpp.hpp>
#include <tinyxml2.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <geometry_msgs/msg/twist.h>
#include <rbs_skill_interfaces/srv/assemble_state.hpp>
#define ASSEMBLE_PREFIX_PARAM_NAME "assemble_prefix"
#define SERVICE_NAME "assemble_state"
#define ASSEMBLE_DIR_PARAM_NAME "assemble_dir"
#define ASSEMBLE_POSITION_OFFSET 0.5
#define ASSEMBLE_ORIENTATION_OFFSET 0.5
#define ASSEMBLE_SDF_PATH(package_dir, assemble_name) \
(package_dir) + "/models/" + (assemble_name) + "/model.sdf"
static inline geometry_msgs::msg::Pose
get_pose(const std::vector<std::string> &tokens) {
geometry_msgs::msg::Pose p;
p.position.set__x(std::stod(tokens.at(0)));
p.position.set__y(std::stod(tokens.at(1)));
p.position.set__z(std::stod(tokens.at(2)));
p.orientation.set__x(std::stod(tokens.at(3)));
p.orientation.set__y(std::stod(tokens.at(4)));
p.orientation.set__z(std::stod(tokens.at(5)));
return p;
}
static inline geometry_msgs::msg::PoseStamped
get_pose_stamped(const std::string &pose_string) {
std::stringstream ss(pose_string);
std::istream_iterator<std::string> begin(ss);
std::istream_iterator<std::string> end;
std::vector<std::string> tokens(begin, end);
geometry_msgs::msg::PoseStamped ps;
ps.set__pose(get_pose(tokens));
return ps;
}
static std::vector<geometry_msgs::msg::PoseStamped>
get_assemble_poses(const std::string &assemble_name,
const std::string &part_name, const std::string &package_dir,
const std::string &assemble_prefix) {
std::vector<geometry_msgs::msg::PoseStamped> result;
std::filesystem::path sdf_path = package_dir + assemble_name + ".sdf";
tinyxml2::XMLDocument doc;
doc.LoadFile(sdf_path.c_str());
auto model = doc.RootElement()->FirstChildElement("model");
auto joint = model->FirstChildElement("joint");
while (joint) {
auto frame_id = joint->FirstChildElement("child")->GetText();
if (frame_id != part_name)
continue;
auto ps = get_pose_stamped(joint->FirstChildElement("pose")->GetText());
ps.header.set__frame_id(assemble_prefix + part_name);
result.push_back(ps);
joint = joint->NextSiblingElement("joint");
}
return result;
}
class AssembleStateServer : public rclcpp::Node {
public:
AssembleStateServer(rclcpp::NodeOptions options)
: Node(SERVICE_NAME,
options.allow_undeclared_parameters(true)
.automatically_declare_parameters_from_overrides(true)) {
assemble_dir_ = get_parameter(ASSEMBLE_DIR_PARAM_NAME).as_string();
assemble_prefix_ = get_parameter(ASSEMBLE_PREFIX_PARAM_NAME).as_string();
tf_buffer_ = std::make_unique<tf2_ros::Buffer>(get_clock());
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
service_ = create_service<rbs_skill_interfaces::srv::AssembleState>(
SERVICE_NAME, std::bind(&AssembleStateServer::callback, this,
std::placeholders::_1, std::placeholders::_2));
}
void
callback(std::shared_ptr<rbs_skill_interfaces::srv::AssembleState::Request>
request,
std::shared_ptr<rbs_skill_interfaces::srv::AssembleState::Response>
response) {
auto state = static_cast<AssembleReqState>(request->req_kind);
bool call_status = false;
std::string assemble_name = request->assemble_name;
std::string part_name = request->part_name;
auto assemble = assembles_.find(request->assemble_name);
if (assemble == assembles_.end())
assembles_.insert(std::make_pair(
request->assemble_name,
Assemble{.part = request->part_name,
.ws = request->workspace,
.state = NONE,
.poses =
get_assemble_poses(assemble_name, part_name,
assemble_dir_, assemble_prefix_)}));
RCLCPP_INFO(get_logger(), "callback call with assemble name: %s",
assemble_name.c_str());
switch (state) {
case NONE:
call_status = false;
break;
case INITIALIZE:
call_status = (assembles_.at(assemble_name).state == NONE) &&
on_initialize(&assembles_.at(assemble_name));
break;
case VALIDATE:
response->validate_status =
(call_status = (assembles_.at(assemble_name).state == INITIALIZE)) &&
on_validate(&assembles_.at(assemble_name));
break;
case COMPLETE:
call_status = (assembles_.at(assemble_name).state == VALIDATE) &&
on_complete(&assembles_.at(assemble_name));
if (call_status)
assembles_.erase(assemble_name);
break;
}
response->call_status = call_status;
response->assemble_name = assemble_name;
}
private:
enum AssembleReqState {
NONE = -1,
INITIALIZE = 0,
VALIDATE = 1,
COMPLETE = 2
};
struct Assemble {
std::string part;
std::string ws;
AssembleReqState state;
std::vector<geometry_msgs::msg::PoseStamped> poses;
std::unique_ptr<tf2_ros::StaticTransformBroadcaster> tf2_broadcaster;
};
bool on_initialize(Assemble *assemble) {
RCLCPP_INFO(get_logger(), "initialize assemble state for part: %s",
assemble->part.c_str());
try {
assemble->tf2_broadcaster =
std::make_unique<tf2_ros::StaticTransformBroadcaster>(this);
} catch (const tf2::TransformException &ex) {
RCLCPP_WARN(get_logger(), "error while create tf2_broadcaster: %s",
ex.what());
return false;
}
assemble->state = INITIALIZE;
for (const auto &it : assemble->poses) {
geometry_msgs::msg::TransformStamped t;
t.header.frame_id = assemble->ws;
t.child_frame_id = it.header.frame_id;
auto pose = it.pose;
t.transform.translation.x = pose.position.x;
t.transform.translation.y = pose.position.y;
t.transform.translation.z = pose.position.z;
t.transform.rotation.x = pose.orientation.x;
t.transform.rotation.y = pose.orientation.y;
t.transform.rotation.z = pose.orientation.z;
t.transform.rotation.w = pose.orientation.w;
assemble->tf2_broadcaster->sendTransform(t);
}
return true;
}
bool on_validate(Assemble *assemble) {
RCLCPP_INFO(get_logger(), "validate assemble state for part: %s",
assemble->part.c_str());
std::string frame_from = assemble_prefix_ + assemble->part;
std::string frame_to = assemble->part;
geometry_msgs::msg::TransformStamped ts;
try {
ts =
tf_buffer_->lookupTransform(frame_to, frame_from, tf2::TimePointZero);
} catch (const tf2::TransformException &ex) {
return false;
}
auto pos_validate = ts.transform.translation.x < ASSEMBLE_POSITION_OFFSET &&
ts.transform.translation.y < ASSEMBLE_POSITION_OFFSET &&
ts.transform.translation.z < ASSEMBLE_POSITION_OFFSET;
auto orient_validate =
ts.transform.rotation.x < ASSEMBLE_ORIENTATION_OFFSET &&
ts.transform.rotation.y < ASSEMBLE_ORIENTATION_OFFSET &&
ts.transform.rotation.z < ASSEMBLE_ORIENTATION_OFFSET;
RCLCPP_INFO(get_logger(), "pos_validate: %d, orient_validate: %d",
pos_validate, orient_validate);
assemble->state = (pos_validate && orient_validate) ? VALIDATE : INITIALIZE;
return assemble->state == VALIDATE;
}
bool on_complete(Assemble *assemble) {
RCLCPP_INFO(get_logger(), "complete assemble state for part: %s",
assemble->part.c_str());
try {
assemble->tf2_broadcaster.reset();
assemble->tf2_broadcaster = NULL;
assemble->poses.clear();
} catch (const std::exception &ex) {
RCLCPP_WARN(get_logger(), "something happen on tf2.reset(): %s",
ex.what());
return false;
}
assemble->state = COMPLETE;
return true;
}
private:
std::map<std::string, Assemble> assembles_;
std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
std::unique_ptr<tf2_ros::StaticTransformBroadcaster> tf2_broadcaster_;
std::mutex mt;
rclcpp::Service<rbs_skill_interfaces::srv::AssembleState>::SharedPtr service_;
std::string assemble_dir_;
std::string assemble_prefix_;
};
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(AssembleStateServer);

1
rbs_skill_servers/src/move_cartesian_path_action_server.cpp
View file

@ -179,6 +179,7 @@ private:
moveit::core::MoveItErrorCode execute_err_code =
move_group_interface.execute(plan);
if (execute_err_code == moveit::core::MoveItErrorCode::SUCCESS) {
result->success = true;
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
} else if (execute_err_code == moveit::core::MoveItErrorCode::FAILURE) {

1
rbs_skill_servers/src/move_topose_action_server.cpp
View file

@ -133,6 +133,7 @@ private:
moveit::core::MoveItErrorCode move_err_code =
move_group_interface.execute(plan);
if (move_err_code == moveit::core::MoveItErrorCode::SUCCESS) {
result->success = true;
goal_handle->succeed(result);
RCLCPP_INFO(this->get_logger(), "Successfully executed action goal");
} else if (move_err_code == moveit::core::MoveItErrorCode::FAILURE) {

244
rbs_skill_servers/src/moveit_update_planning_scene.cpp
View file

@ -1,244 +0,0 @@
#include <filesystem>
#include <iostream>
#include <vector>
#include <geometric_shapes/mesh_operations.h>
#include <geometric_shapes/shape_operations.h>
#include <sdf/World.hh>
#include <sdf/sdf.hh>
#include <tinyxml2.h>
#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/static_transform_broadcaster.h>
#include <tf2_ros/transform_listener.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/msg/collision_object.hpp>
#include "rbs_skill_interfaces/srv/planning_scene_object_state.hpp"
#define SERVICE_NAME "update_planning_scene"
#define INIT_SCENE_PARAM_NAME "init_scene"
#define CUSTOM_MODEL_PATH_PARAM_NAME "models_paths"
static geometry_msgs::msg::Pose
pose_from_pose3d(const ignition::math::Pose3d &pose) {
geometry_msgs::msg::Pose result;
auto position = pose.Pos();
result.position.set__x(position.X());
result.position.set__y(position.Y());
result.position.set__z(position.Z());
auto orientation = pose.Rot();
result.orientation.set__x(orientation.X());
result.orientation.set__y(orientation.Y());
result.orientation.set__z(orientation.Z());
result.orientation.set__w(orientation.W());
return result;
}
static std::string
get_correct_mesh_path(const std::string &uri,
const std::vector<std::string> &resources) {
std::string result = "";
std::regex reg(R"((?:model|package)(?:\:\/)(.*))");
std::smatch m;
if (std::regex_match(uri, m, reg)) {
std::string rel_path = m[1];
std::for_each(resources.begin(), resources.end(),
[&result, &rel_path](const std::string &res) {
if (result.empty())
result = std::filesystem::exists(res + rel_path)
? std::string(res + rel_path)
: result;
});
}
return "file://" + result;
}
static moveit_msgs::msg::CollisionObject
get_object(const sdf::Model *model, const std::vector<std::string> &resources) {
moveit_msgs::msg::CollisionObject obj;
obj.header.frame_id = "world";
obj.id = model->Name();
obj.pose = pose_from_pose3d(model->RawPose());
size_t link_count = model->LinkCount();
for (size_t i = 0; i < link_count; ++i) {
auto link = model->LinkByIndex(i);
auto collision = link->CollisionByIndex(0);
auto link_pose = pose_from_pose3d(link->RawPose());
auto geometry = collision->Geom();
switch (geometry->Type()) {
case sdf::GeometryType::MESH: {
auto path =
get_correct_mesh_path(geometry->MeshShape()->Uri(), resources);
shapes::Mesh *m = shapes::createMeshFromResource(
!path.empty() ? path : geometry->MeshShape()->Uri());
auto scale = geometry->MeshShape()->Scale().X();
m->scale(scale);
Eigen::Vector3d centroid = {0, 0, 0};
for (size_t i = 0; i < 3 * m->vertex_count; i += 3) {
const auto x = m->vertices[i];
const auto y = m->vertices[i + 1];
const auto z = m->vertices[i + 2];
centroid.x() += x * (1 - scale);
centroid.y() += y * (1 - scale);
centroid.z() += z * (1 - scale);
}
centroid = centroid / m->vertex_count;
for (size_t i = 0; i < 3 * m->vertex_count; i += 3) {
m->vertices[i] -= centroid.x();
m->vertices[i + 1] -= centroid.y();
m->vertices[i + 2] -= centroid.z();
}
shape_msgs::msg::Mesh mesh;
shapes::ShapeMsg m_msg;
shapes::constructMsgFromShape(m, m_msg);
mesh = boost::get<shape_msgs::msg::Mesh>(m_msg);
obj.meshes.push_back(mesh);
obj.mesh_poses.push_back(link_pose);
break;
}
case sdf::GeometryType::BOX: {
auto sdf_box = geometry->BoxShape();
shape_msgs::msg::SolidPrimitive box;
box.type = shape_msgs::msg::SolidPrimitive::BOX;
auto sdf_box_size = sdf_box->Size();
box.dimensions.push_back(sdf_box_size.X());
box.dimensions.push_back(sdf_box_size.Y());
box.dimensions.push_back(sdf_box_size.Z());
obj.primitives.push_back(box);
obj.primitive_poses.push_back(link_pose);
break;
}
case sdf::GeometryType::PLANE: {
auto sdf_plane = geometry->PlaneShape();
shape_msgs::msg::Plane plane;
auto normal = sdf_plane->Normal();
plane.coef[0] = normal.X();
plane.coef[1] = normal.Y();
plane.coef[2] = normal.Z();
obj.planes.push_back(plane);
obj.plane_poses.push_back(link_pose);
break;
}
case sdf::GeometryType::EMPTY:
case sdf::GeometryType::CYLINDER:
case sdf::GeometryType::SPHERE:
case sdf::GeometryType::HEIGHTMAP:
case sdf::GeometryType::CAPSULE:
case sdf::GeometryType::ELLIPSOID:
case sdf::GeometryType::POLYLINE:
break;
}
}
return obj;
}
static std::vector<moveit_msgs::msg::CollisionObject>
get_objects(const sdf::World *world, const std::string &model_resources) {
std::vector<std::string> resources;
std::regex reg("\\:+");
std::sregex_token_iterator begin(model_resources.begin(),
model_resources.end(), reg, -1),
end;
std::copy(++begin, end, std::back_inserter(resources));
std::vector<moveit_msgs::msg::CollisionObject> result;
auto models_count = world->ModelCount();
for (size_t i = 0; i < models_count; ++i) {
try {
auto model = world->ModelByIndex(i);
result.push_back(get_object(model, resources));
} catch (std::exception &ex) {
std::cerr << ex.what() << std::endl;
}
}
return result;
}
class UpdatePlanningSceneServer : public rclcpp::Node {
public:
UpdatePlanningSceneServer(rclcpp::NodeOptions options)
: Node("update_planning_scene_node",
options.allow_undeclared_parameters(true)
.automatically_declare_parameters_from_overrides(true)) {
std::string scene = get_parameter(INIT_SCENE_PARAM_NAME).as_string();
std::string model_resources =
get_parameter(CUSTOM_MODEL_PATH_PARAM_NAME).as_string();
if (!scene.empty()) {
RCLCPP_INFO(get_logger(), "Init scene from file: %s", scene.c_str());
init_scene(scene, model_resources);
}
service_ =
create_service<rbs_skill_interfaces::srv::PlanningSceneObjectState>(
SERVICE_NAME,
std::bind(&UpdatePlanningSceneServer::callback, this,
std::placeholders::_1, std::placeholders::_2));
}
bool init_scene(const std::string &init_scene,
const std::string &model_resources) {
sdf::Root root;
sdf::ParserConfig config;
config.AddURIPath("package://", model_resources);
config.AddURIPath("model://", model_resources);
sdf::Errors errors = root.Load(init_scene, config);
if (!errors.empty()) {
for (auto const &e : errors)
RCLCPP_ERROR(get_logger(), "%s", e.Message().c_str());
return false;
}
auto world = root.WorldByIndex(0);
auto objects = get_objects(world, model_resources);
planning_scene_.applyCollisionObjects(objects);
return true;
}
void
callback(std::shared_ptr<
rbs_skill_interfaces::srv::PlanningSceneObjectState::Request>
request,
std::shared_ptr<
rbs_skill_interfaces::srv::PlanningSceneObjectState::Response>
response) {
auto state =
static_cast<UpdatePlanningSceneRequestState>(request->req_kind);
moveit_msgs::msg::CollisionObject obj;
obj.id = request->frame_name;
switch (state) {
case ADD:
case UPDATE:
obj.meshes.resize(1);
obj.mesh_poses.resize(1);
obj.operation = state == ADD ? moveit_msgs::msg::CollisionObject::ADD
: moveit_msgs::msg::CollisionObject::MOVE;
obj.meshes.at(0) = std::move(request->mesh);
obj.mesh_poses.at(0) = std::move(request->pose.pose);
break;
case REMOVE:
obj.operation = moveit_msgs::msg::CollisionObject::REMOVE;
break;
}
response->call_status = planning_scene_.applyCollisionObject(obj);
}
private:
enum UpdatePlanningSceneRequestState { ADD = 0, REMOVE = 1, UPDATE = 2 };
rclcpp::Service<
rbs_skill_interfaces::srv::PlanningSceneObjectState>::SharedPtr service_;
moveit::planning_interface::PlanningSceneInterface planning_scene_;
};
#include "rclcpp_components/register_node_macro.hpp"
RCLCPP_COMPONENTS_REGISTER_NODE(UpdatePlanningSceneServer);

0
rbs_task_planner/COLCON_IGNORE Normal file
View file

7
rbs_utils/rbs_utils/CMakeLists.txt
View file

@ -30,6 +30,13 @@ set(deps
add_library(${PROJECT_NAME} SHARED src/rbs_utils.cpp)
ament_python_install_package(${PROJECT_NAME})
install(PROGRAMS
scripts/assembly_config_service.py
DESTINATION lib/${PROJECT_NAME}
)
install(
DIRECTORY include/
DESTINATION include

0
rbs_utils/rbs_utils/rbs_utils/__init__.py Normal file
View file

85
rbs_utils/rbs_utils/scripts/assembly_config_service.py Executable file
View file

@ -0,0 +1,85 @@
#!/usr/bin/python3
import os
import rclpy
from rclpy.node import Node
import yaml
from geometry_msgs.msg import Point, Pose, Quaternion, PoseArray
from rbs_utils_interfaces.msg import NamedPose, RelativeNamedPose, AssemblyConfig
from rbs_utils_interfaces.srv import GetGraspPose, GetWorkspace
class AssemblyConfigService(Node):
def __init__(self, node_name="assembly_config"):
super().__init__(node_name)
self.declare_parameter("db_folder", "asp-example")
db_folder = self.get_parameter("db_folder").get_parameter_value().string_value
# Parse the YAML file
yaml_file = os.path.join(os.getenv('RBS_ASSEMBLY_DIR', ''), db_folder,'rbs_db.yaml')
self.assembly_config = parse_yaml(yaml_file)
# Create services
self.workspace_service = self.create_service(GetWorkspace, "get_workspace", self.get_workspace_callback)
self.grasp_pose_service = self.create_service(GetGraspPose, "get_grasp_pose", self.get_grasp_pose_callback)
def get_workspace_callback(self, request, response):
response.workspace = self.assembly_config.workspace
response.ok = True
return response
def get_grasp_pose_callback(self, request, response):
response.grasp_pose = self.assembly_config.grasp_pose
response.ok = True
return response
def parse_yaml(yaml_file):
with open(yaml_file, 'r') as file:
data = yaml.safe_load(file)
assets_db = data.get('assets_db', '')
workspace = [Point(**point) for point in data.get('workspace', [])]
absolute_part = []
for part in data.get('absolute_part', []):
position = part['pose']['position']
orientation = part['pose']['orientation']
pose = Pose(position=Point(**position), orientation=Quaternion(**orientation))
absolute_part.append(NamedPose(name=part['name'], pose=pose))
relative_part = []
for part in data.get('relative_part', []):
position = part['pose']['position']
orientation = part['pose']['orientation']
pose = Pose(position=Point(**position), orientation=Quaternion(**orientation))
relative_part.append(RelativeNamedPose(name=part['name'], relative_at=part['relative_at'], pose=pose))
grasp_pose = []
for pose in data.get('grasp_pose', []):
position = pose['pose']['position']
orientation = pose['pose']['orientation']
pose_obj = Pose(position=Point(**position), orientation=Quaternion(**orientation))
grasp_pose.append(RelativeNamedPose(name=pose['name'], relative_at=pose['relative_at'], pose=pose_obj))
extra_poses = []
assembly_config = AssemblyConfig(
assets_db=assets_db,
workspace=workspace,
absolute_part=absolute_part,
relative_part=relative_part,
grasp_pose=grasp_pose,
extra_poses=extra_poses
)
return assembly_config
def main(args=None):
rclpy.init(args=args)
node = AssemblyConfigService()
rclpy.spin(node)
rclpy.shutdown()
if __name__ == '__main__':
main()

2
rbs_utils/rbs_utils_interfaces/CMakeLists.txt
View file

@ -15,6 +15,8 @@ rosidl_generate_interfaces(${PROJECT_NAME}
"msg/AssemblyConfig.msg"
"msg/NamedPose.msg"
"msg/RelativeNamedPose.msg"
"srv/GetGraspPose.srv"
"srv/GetWorkspace.srv"
DEPENDENCIES std_msgs geometry_msgs
)

4
rbs_utils/rbs_utils_interfaces/srv/GetGraspPose.srv Normal file
View file

@ -0,0 +1,4 @@
string model_name
---
rbs_utils_interfaces/RelativeNamedPose[] grasp_pose
bool ok

4
rbs_utils/rbs_utils_interfaces/srv/GetWorkspace.srv Normal file
View file

@ -0,0 +1,4 @@
string type
---
geometry_msgs/Point[] workspace
bool ok

4
repos/sim.rbs.repos
View file

@ -13,8 +13,8 @@ repositories:
version: main
behavior_tree:
type: git
url: https://github.com/solid-sinusoid/behavior_tree.git
version: master
url: https://github.com/BehaviorTree/BehaviorTree.ROS2.git
version: humble
dynamic_message_introspection:
type: git
url: https://github.com/osrf/dynamic_message_introspection.git