diff --git a/rbs_bringup/launch/rbs_robot.launch.py b/rbs_bringup/launch/rbs_robot.launch.py
index 9794b03..5eca9b0 100644
--- a/rbs_bringup/launch/rbs_robot.launch.py
+++ b/rbs_bringup/launch/rbs_robot.launch.py
@@ -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
diff --git a/rbs_bringup/launch/single_robot.launch.py b/rbs_bringup/launch/single_robot.launch.py
index 012c80f..528a81a 100644
--- a/rbs_bringup/launch/single_robot.launch.py
+++ b/rbs_bringup/launch/single_robot.launch.py
@@ -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(
diff --git a/rbs_bt_executor/CMakeLists.txt b/rbs_bt_executor/CMakeLists.txt
index 6036924..7037bec 100644
--- a/rbs_bt_executor/CMakeLists.txt
+++ b/rbs_bt_executor/CMakeLists.txt
@@ -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}
diff --git a/rbs_bt_executor/bt_trees/PosesTest.xml b/rbs_bt_executor/bt_trees/PosesTest.xml
deleted file mode 100644
index c4e655b..0000000
--- a/rbs_bt_executor/bt_trees/PosesTest.xml
+++ /dev/null
@@ -1,21 +0,0 @@
-
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/rbs_bt_executor/bt_trees/assemble.xml b/rbs_bt_executor/bt_trees/assemble.xml
deleted file mode 100644
index f3c7026..0000000
--- a/rbs_bt_executor/bt_trees/assemble.xml
+++ /dev/null
@@ -1,57 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- `
-
-
-
-
-
-
-
-
-
diff --git a/rbs_bt_executor/bt_trees/atomic_skills_xml/assemble.xml b/rbs_bt_executor/bt_trees/atomic_skills_xml/assemble.xml
index aa8f594..a613189 100644
--- a/rbs_bt_executor/bt_trees/atomic_skills_xml/assemble.xml
+++ b/rbs_bt_executor/bt_trees/atomic_skills_xml/assemble.xml
@@ -1,5 +1,5 @@
-
-
+
+
@@ -15,20 +15,20 @@
-
-
+
+
-
-
+
+
-
-
+
+
-
+
-
+
\ No newline at end of file
diff --git a/rbs_bt_executor/bt_trees/atomic_skills_xml/get-part-state.xml b/rbs_bt_executor/bt_trees/atomic_skills_xml/get-part-state.xml
index 276fa5e..fd67ca7 100644
--- a/rbs_bt_executor/bt_trees/atomic_skills_xml/get-part-state.xml
+++ b/rbs_bt_executor/bt_trees/atomic_skills_xml/get-part-state.xml
@@ -1,40 +1,39 @@
-
-
+
+
-
-
+
+
-
-
-
+
+
+
-
-
+
+
-
-
+
+
-
-
+
+
-
-
+
+
-
-
+
+
-
-
+
\ No newline at end of file
diff --git a/rbs_bt_executor/bt_trees/atomic_skills_xml/grasp_part.xml b/rbs_bt_executor/bt_trees/atomic_skills_xml/grasp_part.xml
index 26a55ff..93ab7a4 100644
--- a/rbs_bt_executor/bt_trees/atomic_skills_xml/grasp_part.xml
+++ b/rbs_bt_executor/bt_trees/atomic_skills_xml/grasp_part.xml
@@ -1,42 +1,41 @@
-
-
+
+
-
-
+
+
-
-
+
+
-
-
+
+
-
-
+
+
-
+
-
-
+
+
-
-
+
+
-
-
+
+
-
-
+
\ No newline at end of file
diff --git a/rbs_bt_executor/bt_trees/atomic_skills_xml/grasp_skill.xml b/rbs_bt_executor/bt_trees/atomic_skills_xml/grasp_skill.xml
index 137e521..fcd01f1 100644
--- a/rbs_bt_executor/bt_trees/atomic_skills_xml/grasp_skill.xml
+++ b/rbs_bt_executor/bt_trees/atomic_skills_xml/grasp_skill.xml
@@ -1,38 +1,38 @@
-
-
+
+
-
+
-
-
+
+
-
-
-
+
+
+
-
-
+
+
-
-
+
+
-
-
+
+
-
-
+
+
-
-
+
+
-
+
\ No newline at end of file
diff --git a/rbs_bt_executor/bt_trees/atomic_skills_xml/move.xml b/rbs_bt_executor/bt_trees/atomic_skills_xml/move.xml
index 5b4d3f6..63e6da0 100644
--- a/rbs_bt_executor/bt_trees/atomic_skills_xml/move.xml
+++ b/rbs_bt_executor/bt_trees/atomic_skills_xml/move.xml
@@ -1,7 +1,8 @@
-
+
+
-
+
\ No newline at end of file
diff --git a/rbs_bt_executor/bt_trees/atomic_skills_xml/move_gripper.xml b/rbs_bt_executor/bt_trees/atomic_skills_xml/move_gripper.xml
index 2b6963c..a2047bd 100644
--- a/rbs_bt_executor/bt_trees/atomic_skills_xml/move_gripper.xml
+++ b/rbs_bt_executor/bt_trees/atomic_skills_xml/move_gripper.xml
@@ -1,7 +1,8 @@
-
+
+
-
+
\ No newline at end of file
diff --git a/rbs_bt_executor/bt_trees/atomic_skills_xml/print.xml b/rbs_bt_executor/bt_trees/atomic_skills_xml/print.xml
index 188c6a6..539dce9 100644
--- a/rbs_bt_executor/bt_trees/atomic_skills_xml/print.xml
+++ b/rbs_bt_executor/bt_trees/atomic_skills_xml/print.xml
@@ -1,5 +1,5 @@
-
-
+
+
@@ -9,8 +9,8 @@
-
-
+
+
diff --git a/rbs_bt_executor/bt_trees/atomic_skills_xml/remove.xml b/rbs_bt_executor/bt_trees/atomic_skills_xml/remove.xml
index 0a836c7..cf02c9f 100644
--- a/rbs_bt_executor/bt_trees/atomic_skills_xml/remove.xml
+++ b/rbs_bt_executor/bt_trees/atomic_skills_xml/remove.xml
@@ -1,106 +1,31 @@
-
-
+
+
-
+
-
- server_name="/move_topose"
- server_timeout="10"
- cancel_timeout="5" />
+
-
+
-
- server_name="/move_topose"
- server_timeout="10"
- cancel_timeout="5" />
+
-
+
-
- server_name="/move_topose"
- server_timeout="10"
- cancel_timeout="5" />
-
-
-
-
-
-
+
-
+
-
+
-
+
@@ -108,37 +33,37 @@
-
+
-
-
-
-
+
+
+
+
-
-
+
+
-
-
+
+
-
-
-
+
+
+
-
-
-
-
+
+
+
+
-
+
\ No newline at end of file
diff --git a/rbs_bt_executor/bt_trees/bt_movetopose.xml b/rbs_bt_executor/bt_trees/bt_movetopose.xml
deleted file mode 100644
index 68be359..0000000
--- a/rbs_bt_executor/bt_trees/bt_movetopose.xml
+++ /dev/null
@@ -1,17 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/rbs_bt_executor/bt_trees/bt_pe_stop.xml b/rbs_bt_executor/bt_trees/bt_pe_stop.xml
deleted file mode 100644
index ffa643e..0000000
--- a/rbs_bt_executor/bt_trees/bt_pe_stop.xml
+++ /dev/null
@@ -1,18 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/rbs_bt_executor/bt_trees/bt_pe_test.xml b/rbs_bt_executor/bt_trees/bt_pe_test.xml
deleted file mode 100644
index 9580ebe..0000000
--- a/rbs_bt_executor/bt_trees/bt_pe_test.xml
+++ /dev/null
@@ -1,18 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/rbs_bt_executor/bt_trees/bt_template.xml b/rbs_bt_executor/bt_trees/bt_template.xml
deleted file mode 100644
index 197939a..0000000
--- a/rbs_bt_executor/bt_trees/bt_template.xml
+++ /dev/null
@@ -1,22 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/rbs_bt_executor/bt_trees/example_repeat.xml b/rbs_bt_executor/bt_trees/example_repeat.xml
deleted file mode 100644
index 9625029..0000000
--- a/rbs_bt_executor/bt_trees/example_repeat.xml
+++ /dev/null
@@ -1,19 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/rbs_bt_executor/bt_trees/example_skills/env_manager_starter.xml b/rbs_bt_executor/bt_trees/example_skills/env_manager_starter.xml
index 41fd05c..3c01e8c 100644
--- a/rbs_bt_executor/bt_trees/example_skills/env_manager_starter.xml
+++ b/rbs_bt_executor/bt_trees/example_skills/env_manager_starter.xml
@@ -1,11 +1,6 @@
-
-
-
+
+
+
diff --git a/rbs_bt_executor/bt_trees/example_skills/move_to_joint_states.xml b/rbs_bt_executor/bt_trees/example_skills/move_to_joint_states.xml
index b07ff12..f8d49b3 100644
--- a/rbs_bt_executor/bt_trees/example_skills/move_to_joint_states.xml
+++ b/rbs_bt_executor/bt_trees/example_skills/move_to_joint_states.xml
@@ -1,11 +1,6 @@
-
-
-
+
+
+
@@ -20,4 +15,4 @@
-
+
\ No newline at end of file
diff --git a/rbs_bt_executor/bt_trees/example_skills/move_to_pose.xml b/rbs_bt_executor/bt_trees/example_skills/move_to_pose.xml
index 54db9b6..22cb24f 100644
--- a/rbs_bt_executor/bt_trees/example_skills/move_to_pose.xml
+++ b/rbs_bt_executor/bt_trees/example_skills/move_to_pose.xml
@@ -1,11 +1,6 @@
-
-
-
+
+
+
@@ -20,4 +15,4 @@
-
+
\ No newline at end of file
diff --git a/rbs_bt_executor/bt_trees/example_skills/move_to_pose_array.xml b/rbs_bt_executor/bt_trees/example_skills/move_to_pose_array.xml
index c9e3414..76c80ea 100644
--- a/rbs_bt_executor/bt_trees/example_skills/move_to_pose_array.xml
+++ b/rbs_bt_executor/bt_trees/example_skills/move_to_pose_array.xml
@@ -1,12 +1,6 @@
-
-
-
+
+
+
@@ -23,4 +17,4 @@
-
+
\ No newline at end of file
diff --git a/rbs_bt_executor/bt_trees/find_object.xml b/rbs_bt_executor/bt_trees/find_object.xml
deleted file mode 100644
index 914e894..0000000
--- a/rbs_bt_executor/bt_trees/find_object.xml
+++ /dev/null
@@ -1,30 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
- Load from outside
-
-
-
-
-
diff --git a/rbs_bt_executor/bt_trees/nodes_interfaces/general.xml b/rbs_bt_executor/bt_trees/nodes_interfaces/general.xml
index f6f8c90..165b9b8 100644
--- a/rbs_bt_executor/bt_trees/nodes_interfaces/general.xml
+++ b/rbs_bt_executor/bt_trees/nodes_interfaces/general.xml
@@ -1,4 +1,5 @@
-
+
+
diff --git a/rbs_bt_executor/bt_trees/pick_and_place_modified_test_xml.xml b/rbs_bt_executor/bt_trees/pick_and_place_modified_test_xml.xml
deleted file mode 100644
index 27670ad..0000000
--- a/rbs_bt_executor/bt_trees/pick_and_place_modified_test_xml.xml
+++ /dev/null
@@ -1,61 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/rbs_bt_executor/bt_trees/pick_and_place_test.xml b/rbs_bt_executor/bt_trees/pick_and_place_test.xml
deleted file mode 100644
index be47ec1..0000000
--- a/rbs_bt_executor/bt_trees/pick_and_place_test.xml
+++ /dev/null
@@ -1,50 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
diff --git a/rbs_bt_executor/bt_trees/test_gripper.xml b/rbs_bt_executor/bt_trees/test_gripper.xml
deleted file mode 100644
index d449ad1..0000000
--- a/rbs_bt_executor/bt_trees/test_gripper.xml
+++ /dev/null
@@ -1,14 +0,0 @@
-
-
-
-
-
-
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/rbs_bt_executor/bt_trees/test_objdet_cleanup.xml b/rbs_bt_executor/bt_trees/test_objdet_cleanup.xml
deleted file mode 100644
index 4476907..0000000
--- a/rbs_bt_executor/bt_trees/test_objdet_cleanup.xml
+++ /dev/null
@@ -1,17 +0,0 @@
-
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/rbs_bt_executor/bt_trees/test_objdet_run.xml b/rbs_bt_executor/bt_trees/test_objdet_run.xml
deleted file mode 100644
index 6d4f818..0000000
--- a/rbs_bt_executor/bt_trees/test_objdet_run.xml
+++ /dev/null
@@ -1,17 +0,0 @@
-
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/rbs_bt_executor/bt_trees/test_roboclone.xml b/rbs_bt_executor/bt_trees/test_roboclone.xml
index 6fb5741..626ebaf 100644
--- a/rbs_bt_executor/bt_trees/test_roboclone.xml
+++ b/rbs_bt_executor/bt_trees/test_roboclone.xml
@@ -1,36 +1,27 @@
-
-
-
-
+
+
+
-
-
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/rbs_bt_executor/bt_trees/test_tree.xml b/rbs_bt_executor/bt_trees/test_tree.xml
index 8cccbf3..6c63706 100644
--- a/rbs_bt_executor/bt_trees/test_tree.xml
+++ b/rbs_bt_executor/bt_trees/test_tree.xml
@@ -1,36 +1,11 @@
-
-
-
-
+
+
+
-
-
+
+
+
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
diff --git a/rbs_bt_executor/bt_trees/test_tree_env_manager.xml b/rbs_bt_executor/bt_trees/test_tree_env_manager.xml
deleted file mode 100644
index 9cc366e..0000000
--- a/rbs_bt_executor/bt_trees/test_tree_env_manager.xml
+++ /dev/null
@@ -1,12 +0,0 @@
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/rbs_bt_executor/bt_trees/test_tree_main.xml b/rbs_bt_executor/bt_trees/test_tree_main.xml
index 82ecb32..58f50e5 100644
--- a/rbs_bt_executor/bt_trees/test_tree_main.xml
+++ b/rbs_bt_executor/bt_trees/test_tree_main.xml
@@ -1,149 +1,72 @@
-
-
+
+
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
-
-
-
-
-
-
-
-
-
-
+
+
+
+
+
+
+
+
+
+
@@ -153,8 +76,8 @@
-
-
+
+
diff --git a/rbs_bt_executor/bt_trees/test_tree_pe_cleanup.xml b/rbs_bt_executor/bt_trees/test_tree_pe_cleanup.xml
deleted file mode 100644
index 030cd42..0000000
--- a/rbs_bt_executor/bt_trees/test_tree_pe_cleanup.xml
+++ /dev/null
@@ -1,17 +0,0 @@
-
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/rbs_bt_executor/bt_trees/test_tree_pe_configure.xml b/rbs_bt_executor/bt_trees/test_tree_pe_configure.xml
deleted file mode 100644
index 24a4921..0000000
--- a/rbs_bt_executor/bt_trees/test_tree_pe_configure.xml
+++ /dev/null
@@ -1,17 +0,0 @@
-
-
-
-
-
-
-
-
-
\ No newline at end of file
diff --git a/rbs_bt_executor/bt_trees/workspace_movement.xml b/rbs_bt_executor/bt_trees/workspace_movement.xml
new file mode 100644
index 0000000..7b5cae7
--- /dev/null
+++ b/rbs_bt_executor/bt_trees/workspace_movement.xml
@@ -0,0 +1,17 @@
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/rbs_bt_executor/config/bt_executor.yaml b/rbs_bt_executor/config/bt_executor.yaml
new file mode 100644
index 0000000..83bcbeb
--- /dev/null
+++ b/rbs_bt_executor/config/bt_executor.yaml
@@ -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
diff --git a/rbs_bt_executor/include/rbs_bt_executor/lifecycle_node_bt.hpp b/rbs_bt_executor/include/rbs_bt_executor/lifecycle_node_bt.hpp
new file mode 100644
index 0000000..e69de29
diff --git a/rbs_bt_executor/launch/rbs_executor.launch.py b/rbs_bt_executor/launch/rbs_executor.launch.py
index 68f26f1..fc7377f 100644
--- a/rbs_bt_executor/launch/rbs_executor.launch.py
+++ b/rbs_bt_executor/launch/rbs_executor.launch.py
@@ -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)
diff --git a/rbs_bt_executor/package.xml b/rbs_bt_executor/package.xml
index 9e8af79..5b69383 100644
--- a/rbs_bt_executor/package.xml
+++ b/rbs_bt_executor/package.xml
@@ -11,7 +11,7 @@
rbs_utils
rbs_skill_interfaces
env_manager_interfaces
- behavior_tree
+ behaviortree_ros2
ament_lint_auto
ament_lint_common
diff --git a/rbs_bt_executor/src/AddPlanningSceneObject.cpp b/rbs_bt_executor/src/AddPlanningSceneObject.cpp
deleted file mode 100644
index ebde704..0000000
--- a/rbs_bt_executor/src/AddPlanningSceneObject.cpp
+++ /dev/null
@@ -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 {
-public:
- GetPickPlacePose(const std::string &name, const BT::NodeConfiguration &config)
- : BtService(name, config) {
- RCLCPP_INFO(_node->get_logger(), "Start the node");
- }
-
- AddPlanningSceneObjectClient::Request::SharedPtr populate_request() override {
- auto request = std::make_shared();
- RCLCPP_INFO(_node->get_logger(), "Start populate_request()");
- object_name_ = getInput("ObjectName").value();
- auto place_pose_ = getInput>("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("ObjectName"),
- InputPort>("pose"),
- });
- }
-
-private:
- std::string object_name_{};
-};
-
-BT_REGISTER_NODES(factory) {
- factory.registerNodeType("AddPlanningSceneObject");
-}
diff --git a/rbs_bt_executor/src/AssembleProcessState.cpp b/rbs_bt_executor/src/AssembleProcessState.cpp
deleted file mode 100644
index 14f5eec..0000000
--- a/rbs_bt_executor/src/AssembleProcessState.cpp
+++ /dev/null
@@ -1,61 +0,0 @@
-#include
-#include
-#include
-
-#include "rbs_skill_interfaces/srv/assemble_state.hpp"
-
-using AssembleStateSrv = rbs_skill_interfaces::srv::AssembleState;
-
-class AssembleState : public BtService {
-public:
- AssembleState(const std::string &name, const BT::NodeConfiguration &config)
- : BtService(name, config) {
- RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
- }
-
- AssembleStateSrv::Request::SharedPtr populate_request() {
- auto request = std::make_shared();
- auto state_kind = getInput("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("AssembleName").value();
- auto part_name = getInput("PartName").value();
- auto workspace = getInput("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("StateKind"),
- BT::InputPort("AssembleName"),
- BT::InputPort("PartName"),
- BT::InputPort("WorkspaceName")});
- }
-
-private:
- std::map assemble_states_;
-};
-
-#include "behaviortree_cpp_v3/bt_factory.h"
-
-BT_REGISTER_NODES(factory) {
- factory.registerNodeType("AssembleState");
-}
diff --git a/rbs_bt_executor/src/EnvManager.cpp b/rbs_bt_executor/src/EnvManager.cpp
index 70da1dd..5e69775 100644
--- a/rbs_bt_executor/src/EnvManager.cpp
+++ b/rbs_bt_executor/src/EnvManager.cpp
@@ -1,59 +1,43 @@
-#include
-#include
-#include
-#include
-#include
-#include
-#include
-#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
+#include "behaviortree_ros2/plugins.hpp"
+#include
-class EnvManagerStarter : public BtService {
+using EnvManagerService = env_manager_interfaces::srv::StartEnv;
+using namespace BT;
+
+class EnvManager : public RosServiceNode {
public:
- EnvManagerStarter(const std::string &name,
- const BT::NodeConfiguration &config)
- : BtService(name, config) {
- RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
- }
+ EnvManager(const std::string &name, const NodeConfig &conf,
+ const RosNodeParams ¶ms)
+ : RosServiceNode(name, conf, params) {}
- EnvStarterService::Request::SharedPtr populate_request() override {
- auto request = std::make_shared();
- std::string env_name = getInput("env_name").value();
- std::string env_class = getInput("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("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("env_name"),
- BT::InputPort("env_class"),
- BT::OutputPort("workspace"),
+ InputPort("env_name"),
+ InputPort("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("EnvStarter");
-}
diff --git a/rbs_bt_executor/src/GetPickPlacePoses.cpp b/rbs_bt_executor/src/GetPickPlacePoses.cpp
deleted file mode 100644
index 7563ea7..0000000
--- a/rbs_bt_executor/src/GetPickPlacePoses.cpp
+++ /dev/null
@@ -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 {
-public:
- GetPickPlacePose(const std::string &name, const BT::NodeConfiguration &config)
- : BtService(name, config) {}
-
- GetPickPlacePoseClient::Request::SharedPtr populate_request() override {
- auto request = std::make_shared();
- RCLCPP_INFO(_node->get_logger(), "Start populate_request()");
- object_name_ = getInput("ObjectName").value();
- grasp_direction_str_ = getInput("GraspDirection").value();
- place_direction_str_ = getInput("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("GraspPose", response->grasp[0]);
- setOutput("PreGraspPose", response->grasp[1]);
- setOutput("PostGraspPose", response->grasp[2]);
-
- setOutput("PlacePose", response->place[0]);
- setOutput("PrePlacePose", response->place[1]);
- setOutput("PostPlacePose", response->place[2]);
- return BT::NodeStatus::SUCCESS;
- }
-
- static PortsList providedPorts() {
- return providedBasicPorts({
- InputPort("ObjectName"),
- InputPort("GraspDirection"), // x or y or z
- InputPort("PlaceDirection"), // x or y or z
- OutputPort("GraspPose"),
- OutputPort("PreGraspPose"),
- OutputPort("PostGraspPose"),
- // TODO: change to std::vector<>
- OutputPort("PostPostGraspPose"),
- OutputPort("PlacePose"),
- OutputPort("PrePlacePose"),
- OutputPort("PostPlacePose"),
- });
- }
-
- geometry_msgs::msg::Vector3 convert_direction_from_string(std::string str) {
- std::map 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("GetPickPlacePoses");
-}
diff --git a/rbs_bt_executor/src/GetWorkspace.cpp b/rbs_bt_executor/src/GetWorkspace.cpp
new file mode 100644
index 0000000..2aaae68
--- /dev/null
+++ b/rbs_bt_executor/src/GetWorkspace.cpp
@@ -0,0 +1,65 @@
+#include "behaviortree_ros2/bt_service_node.hpp"
+
+#include "rbs_utils_interfaces/srv/get_workspace.hpp"
+#include
+#include "behaviortree_ros2/plugins.hpp"
+#include
+#include
+#include
+#include
+
+using GetWorkspaceService = rbs_utils_interfaces::srv::GetWorkspace;
+using namespace BT;
+
+class GetWorkspace : public RosServiceNode {
+public:
+ GetWorkspace(const std::string &name, const NodeConfig &conf,
+ const RosNodeParams ¶ms)
+ : RosServiceNode(name, conf, params) {}
+
+ static PortsList providedPorts() {
+ return providedBasicPorts({
+ InputPort("type"),
+ OutputPort>("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();
+ auto quat = std::make_shared();
+
+ 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");
+
diff --git a/rbs_bt_executor/src/MoveGripper.cpp b/rbs_bt_executor/src/MoveGripper.cpp
new file mode 100644
index 0000000..6e020d0
--- /dev/null
+++ b/rbs_bt_executor/src/MoveGripper.cpp
@@ -0,0 +1,51 @@
+#include "behaviortree_ros2/bt_action_node.hpp"
+#include "rbs_skill_interfaces/action/gripper_command.hpp"
+#include
+#include
+#include
+
+using GripperCommand = rbs_skill_interfaces::action::GripperCommand;
+using namespace BT;
+
+class GripperControl : public RosActionNode {
+public:
+ GripperControl(const std::string &name, const NodeConfig &conf,
+ const RosNodeParams ¶ms)
+ : RosActionNode(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("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");
diff --git a/rbs_bt_executor/src/MoveToJointStates.cpp b/rbs_bt_executor/src/MoveToJointStates.cpp
index 815a427..90fc558 100644
--- a/rbs_bt_executor/src/MoveToJointStates.cpp
+++ b/rbs_bt_executor/src/MoveToJointStates.cpp
@@ -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
+#include
+#include
+#include
+#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 {
+class MoveToJointState : public RosActionNode {
public:
- MoveToJointState(const std::string &name, const BT::NodeConfiguration &config)
- : BtAction(name, config) {}
-
- MoveToJointStateAction::Goal populate_goal() override {
- getInput("robot_name", robot_name_);
- getInput>("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 ¶ms)
+ : RosActionNode(name, conf, params) {}
static PortsList providedPorts() {
- return providedBasicPorts(
- {InputPort("robot_name"),
- InputPort>("JointState")});
+ return providedBasicPorts({InputPort("robot_name"),
+ InputPort>("JointState")});
}
-private:
- std::string robot_name_{};
- std::vector 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");
-}
\ No newline at end of file
+ NodeStatus onResultReceived(const WrappedResult& wr) override {
+
+ if (!wr.result->success) {
+ return NodeStatus::FAILURE;
+ }
+ return NodeStatus::SUCCESS;
+ }
+};
+
+CreateRosNodePlugin(MoveToJointState, "MoveToJointState");
diff --git a/rbs_bt_executor/src/MoveToPose.cpp b/rbs_bt_executor/src/MoveToPose.cpp
index bed2dfe..066c4d7 100644
--- a/rbs_bt_executor/src/MoveToPose.cpp
+++ b/rbs_bt_executor/src/MoveToPose.cpp
@@ -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
+#include
+#include
-#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 {
- public:
- MoveToPose(const std::string &name, const BT::NodeConfiguration &config)
- : BtAction(name, config) {}
-
- MoveToPoseAction::Goal populate_goal() override {
- getInput("robot_name", robot_name_);
- getInput("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 {
+public:
+ MoveToPose(const std::string &name, const NodeConfig &conf,
+ const RosNodeParams ¶ms)
+ : RosActionNode(name, conf, params) {}
static BT::PortsList providedPorts() {
return providedBasicPorts(
{BT::InputPort("robot_name"),
- BT::InputPort("pose")});
+ BT::InputPort>("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");
-}
+ NodeStatus onResultReceived(const WrappedResult& wr) override {
+ if (!wr.result->success) {
+ return NodeStatus::FAILURE;
+ }
+ return NodeStatus::SUCCESS;
+ }
+
+};
+
+CreateRosNodePlugin(MoveToPose, "MoveToPose");
diff --git a/rbs_bt_executor/src/MoveToPoseArray.cpp b/rbs_bt_executor/src/MoveToPoseArray.cpp
index a8f0d60..4e734da 100644
--- a/rbs_bt_executor/src/MoveToPoseArray.cpp
+++ b/rbs_bt_executor/src/MoveToPoseArray.cpp
@@ -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
+#include
+#include
+#include
#include
-#include
-#include
+using namespace BT;
using MoveToPoseArrayAction = rbs_skill_interfaces::action::MoveitSendPose;
-class MoveToPoseArray : public BtAction {
+class MoveToPoseArray : public RosActionNode {
public:
- MoveToPoseArray(const std::string &name, const BT::NodeConfiguration &config)
- : BtAction(name, config) {
- RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
- }
+ MoveToPoseArray(const std::string &name, const NodeConfig &conf,
+ const RosNodeParams ¶ms)
+ : RosActionNode(name, conf, params) {}
- MoveToPoseArrayAction::Goal populate_goal() override {
- auto goal = MoveToPoseArrayAction::Goal();
- getInput("robot_name", robot_name_);
- getInput("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("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("robot_name"),
- BT::InputPort("pose_vec_in"),
- BT::OutputPort("pose_vec_out")});
+ {InputPort("robot_name"),
+ InputPort>(
+ "pose_vec_in"),
+ OutputPort>(
+ "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 m_pose_vec;
};
-BT_REGISTER_NODES(factory) {
- factory.registerNodeType("MoveToPoseArray");
-}
+CreateRosNodePlugin(MoveToPoseArray, "MoveToPoseArray");
diff --git a/rbs_bt_executor/src/ObjectDetection.cpp b/rbs_bt_executor/src/ObjectDetection.cpp
index e1bb8ae..b010271 100644
--- a/rbs_bt_executor/src/ObjectDetection.cpp
+++ b/rbs_bt_executor/src/ObjectDetection.cpp
@@ -1,69 +1,64 @@
-#include
-#include
-
-#include
-#include
-#include
-
-#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
+#include
+#include
+#include
+#include
+using namespace BT;
+using namespace std::chrono_literals;
using ObjDetectionSrv = lifecycle_msgs::srv::ChangeState;
-class ObjectDetection : public BtService {
+
+class ObjectDetection : public RosServiceNode {
public:
- ObjectDetection(const std::string &name, const BT::NodeConfiguration &config)
- : BtService(name, config) {
- RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
-
- _set_params_client = std::make_shared(
- _node, "/object_detection");
-
- while (!_set_params_client->wait_for_service(1s)) {
+ ObjectDetection(const std::string &name, const NodeConfig &conf,
+ const RosNodeParams ¶ms)
+ : RosServiceNode(name, conf, params) {
+ m_params_client = std::make_shared(
+ 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("SettingPath").value();
+ getInput("SettingPath", m_settings_path);
}
- ObjDetectionSrv::Request::SharedPtr populate_request() {
- auto request = std::make_shared();
- _req_type = getInput("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("ReqKind"),
- BT::InputPort("SettingPath"),
+ InputPort("ReqKind"),
+ InputPort("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 _set_params_client;
+ std::string m_settings_path{};
+ // std::string _model_path{};
+ std::string m_req_type{};
+ std::shared_ptr m_params_client;
std::vector _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 _parameters{
- rclcpp::Parameter("setting_path", _setting_path)};
- _set_params_client->set_parameters(_parameters);
+ std::vector 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 _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");
-}
+CreateRosNodePlugin(ObjectDetection, "ObjectDetection");
diff --git a/rbs_bt_executor/src/PoseEstimation.cpp b/rbs_bt_executor/src/PoseEstimation.cpp
index 0a6b183..939e0d6 100644
--- a/rbs_bt_executor/src/PoseEstimation.cpp
+++ b/rbs_bt_executor/src/PoseEstimation.cpp
@@ -1,89 +1,94 @@
-#include
-#include
-
-#include
-#include
-#include
+#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
+#include
+using namespace BT;
+using namespace std::chrono_literals;
using PoseEstimationSrv = lifecycle_msgs::srv::ChangeState;
-class PoseEstimation : public BtService {
+
+class PoseEstimation : public RosServiceNode {
public:
- PoseEstimation(const std::string &name, const BT::NodeConfiguration &config)
- : BtService(name, config) {
- RCLCPP_INFO_STREAM(_node->get_logger(), "Start node.");
+ PoseEstimation(const std::string &name, const NodeConfig &conf,
+ const RosNodeParams ¶ms)
+ : RosServiceNode(name, conf, params) {
+ RCLCPP_INFO_STREAM(logger(), "Start node.");
- _set_params_client = std::make_shared(
- _node, "/pose_estimation");
+ m_params_client = std::make_shared(
+ 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("ObjectName").value();
+ // Get model name paramter from BT ports
+ getInput("ObjectName", m_model_name);
}
- PoseEstimationSrv::Request::SharedPtr populate_request() {
- auto request = std::make_shared();
- _req_type = getInput("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("ReqKind"),
- BT::InputPort("ObjectName"),
- BT::InputPort("ObjectPath"),
+ InputPort("ReqKind"),
+ InputPort("ObjectName"),
+ InputPort("ObjectPath"),
});
}
private:
- uint8_t transition_id_{};
- std::string _model_name{};
- std::string _model_path{};
- std::string _req_type{};
- std::shared_ptr _set_params_client;
- std::vector _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 m_params_client;
+ std::vector 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("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 _parameters{
- rclcpp::Parameter("mesh_path", _model_name)};
- _set_params_client->set_parameters(_parameters);
+ std::vector 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 _parameters{
- rclcpp::Parameter("mesh_path", _model_name)};
- _set_params_client->set_parameters(_parameters);
+ std::vector 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");
-}
+CreateRosNodePlugin(PoseEstimation, "PoseEstimation");
diff --git a/rbs_bt_executor/src/TreeRunner.cpp b/rbs_bt_executor/src/TreeRunner.cpp
new file mode 100644
index 0000000..0fc5b38
--- /dev/null
+++ b/rbs_bt_executor/src/TreeRunner.cpp
@@ -0,0 +1,43 @@
+#include
+#include
+
+// #include
+// #include
+
+class RbsBtExecutor : public BT::TreeExecutionServer {
+public:
+ RbsBtExecutor(const rclcpp::NodeOptions &options)
+ : TreeExecutionServer(options) {}
+
+ void onTreeCreated(BT::Tree &tree) override {
+ logger_cout_ = std::make_shared(tree);
+ }
+
+ std::optional
+ 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 logger_cout_;
+ // rclcpp::Subscription::SharedPtr sub_;
+};
+
+int main(int argc, char *argv[]) {
+ rclcpp::init(argc, argv);
+
+ rclcpp::NodeOptions options;
+ auto action_server = std::make_shared(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();
+}
diff --git a/rbs_bt_executor/src/gripper_move.cpp b/rbs_bt_executor/src/gripper_move.cpp
deleted file mode 100644
index 855bdbf..0000000
--- a/rbs_bt_executor/src/gripper_move.cpp
+++ /dev/null
@@ -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 {
-
-public:
- GripperControl(const std::string &name, const BT::NodeConfiguration &config)
- : BtAction(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("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("pose")});
- }
-
-private:
- struct {
- double open = 0.008;
- double close = 0.000;
- } position;
-
- std::string pose;
-};
-
-BT_REGISTER_NODES(factory) {
- factory.registerNodeType("GripperControl");
-}
\ No newline at end of file
diff --git a/rbs_bt_executor/src/rbsBTAction.cpp b/rbs_bt_executor/src/rbsBTAction.cpp
index 4ecd000..e28bd09 100644
--- a/rbs_bt_executor/src/rbsBTAction.cpp
+++ b/rbs_bt_executor/src/rbsBTAction.cpp
@@ -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
+#include
+#include
#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 {
+class RbsBtAction : public RosServiceNode {
public:
- RbsBtAction(const std::string &name, const BT::NodeConfiguration &config)
- : BtService(name, config) {
+ RbsBtAction(const std::string &name, const NodeConfig& conf, const RosNodeParams& params)
+ : RosServiceNode(name, conf, params) {
- _action_name = getInput(STR_ACTION).value();
- RCLCPP_INFO_STREAM(_node->get_logger(), "Start node RbsBtAction: " + _action_name);
+ m_action_name = getInput(STR_ACTION).value();
+ RCLCPP_INFO_STREAM(logger(), "Start node RbsBtAction: " + m_action_name);
- _set_params_client = std::make_shared(_node, NODE_NAME);
+ m_params_client = std::make_shared(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();
- request->action = _action_name;
- request->sid = getInput(STR_SID).value();
- request->command = getInput(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(STR_SID),
- BT::InputPort(STR_ACTION),
- BT::InputPort(STR_COMMAND)
+ InputPort(STR_SID),
+ InputPort(STR_ACTION),
+ InputPort(STR_COMMAND)
});
}
private:
- std::string _action_name{};
- std::shared_ptr _set_params_client;
+ std::string m_action_name{};
+ std::shared_ptr m_params_client;
};
-#include "behaviortree_cpp_v3/bt_factory.h"
-
-BT_REGISTER_NODES(factory) {
- factory.registerNodeType("RbsBtAction");
-}
+CreateRosNodePlugin(RbsBtAction, "RbsAction")
diff --git a/rbs_simulation/launch/simulation_gazebo.launch.py b/rbs_simulation/launch/simulation_gazebo.launch.py
index d69f715..8c70fb4 100644
--- a/rbs_simulation/launch/simulation_gazebo.launch.py
+++ b/rbs_simulation/launch/simulation_gazebo.launch.py
@@ -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(
diff --git a/rbs_skill_servers/CMakeLists.txt b/rbs_skill_servers/CMakeLists.txt
index d30c81d..f92b1ed 100644
--- a/rbs_skill_servers/CMakeLists.txt
+++ b/rbs_skill_servers/CMakeLists.txt
@@ -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 $
- $