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 $ - $) -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 $ +# $) +# 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}) diff --git a/rbs_skill_servers/src/add_planning_scene_objects_service.cpp b/rbs_skill_servers/src/add_planning_scene_objects_service.cpp deleted file mode 100644 index 008107d..0000000 --- a/rbs_skill_servers/src/add_planning_scene_objects_service.cpp +++ /dev/null @@ -1,72 +0,0 @@ -#include "moveit/move_group_interface/move_group_interface.h" -#include "rbs_skill_interfaces/srv/add_planning_scene_object.hpp" -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" - -using AddPlanningSceneObject = - rbs_skill_interfaces::srv::AddPlanningSceneObject; -rclcpp::Node::SharedPtr g_node = nullptr; - -void handle_service( - const std::shared_ptr request_header, - const std::shared_ptr request, - const std::shared_ptr 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( - "add_planing_scene_object_service", handle_service); - rclcpp::spin(g_node); - rclcpp::shutdown(); - g_node = nullptr; - return 0; -} diff --git a/rbs_skill_servers/src/assemble_state_server.cpp b/rbs_skill_servers/src/assemble_state_server.cpp deleted file mode 100644 index 25ec2c5..0000000 --- a/rbs_skill_servers/src/assemble_state_server.cpp +++ /dev/null @@ -1,245 +0,0 @@ -#include -#include -#include -#include -#include - -#include -#include - -#include -#include -#include - -#include -#include -#include - -#include - -#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 &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 begin(ss); - std::istream_iterator end; - std::vector tokens(begin, end); - - geometry_msgs::msg::PoseStamped ps; - ps.set__pose(get_pose(tokens)); - return ps; -} - -static std::vector -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 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(get_clock()); - tf_listener_ = std::make_shared(*tf_buffer_); - service_ = create_service( - SERVICE_NAME, std::bind(&AssembleStateServer::callback, this, - std::placeholders::_1, std::placeholders::_2)); - } - - void - callback(std::shared_ptr - request, - std::shared_ptr - response) { - auto state = static_cast(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 poses; - std::unique_ptr 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(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 assembles_; - std::shared_ptr tf_listener_; - std::unique_ptr tf_buffer_; - std::unique_ptr tf2_broadcaster_; - std::mutex mt; - rclcpp::Service::SharedPtr service_; - std::string assemble_dir_; - std::string assemble_prefix_; -}; - -#include "rclcpp_components/register_node_macro.hpp" - -RCLCPP_COMPONENTS_REGISTER_NODE(AssembleStateServer); diff --git a/rbs_skill_servers/src/move_cartesian_path_action_server.cpp b/rbs_skill_servers/src/move_cartesian_path_action_server.cpp index 69d63c4..989c8f9 100644 --- a/rbs_skill_servers/src/move_cartesian_path_action_server.cpp +++ b/rbs_skill_servers/src/move_cartesian_path_action_server.cpp @@ -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) { diff --git a/rbs_skill_servers/src/move_topose_action_server.cpp b/rbs_skill_servers/src/move_topose_action_server.cpp index aba0780..553bb53 100644 --- a/rbs_skill_servers/src/move_topose_action_server.cpp +++ b/rbs_skill_servers/src/move_topose_action_server.cpp @@ -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) { diff --git a/rbs_skill_servers/src/moveit_update_planning_scene.cpp b/rbs_skill_servers/src/moveit_update_planning_scene.cpp deleted file mode 100644 index 8840992..0000000 --- a/rbs_skill_servers/src/moveit_update_planning_scene.cpp +++ /dev/null @@ -1,244 +0,0 @@ -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -#include -#include - -#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 &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 &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(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 -get_objects(const sdf::World *world, const std::string &model_resources) { - std::vector 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 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( - 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(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); diff --git a/rbs_task_planner/COLCON_IGNORE b/rbs_task_planner/COLCON_IGNORE new file mode 100644 index 0000000..e69de29 diff --git a/rbs_utils/rbs_utils/CMakeLists.txt b/rbs_utils/rbs_utils/CMakeLists.txt index a07a399..438774a 100644 --- a/rbs_utils/rbs_utils/CMakeLists.txt +++ b/rbs_utils/rbs_utils/CMakeLists.txt @@ -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 diff --git a/rbs_utils/rbs_utils/rbs_utils/__init__.py b/rbs_utils/rbs_utils/rbs_utils/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/rbs_utils/rbs_utils/scripts/assembly_config_service.py b/rbs_utils/rbs_utils/scripts/assembly_config_service.py new file mode 100755 index 0000000..56b42f7 --- /dev/null +++ b/rbs_utils/rbs_utils/scripts/assembly_config_service.py @@ -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() diff --git a/rbs_utils/rbs_utils_interfaces/CMakeLists.txt b/rbs_utils/rbs_utils_interfaces/CMakeLists.txt index 0011e8d..a1b393d 100644 --- a/rbs_utils/rbs_utils_interfaces/CMakeLists.txt +++ b/rbs_utils/rbs_utils_interfaces/CMakeLists.txt @@ -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 ) diff --git a/rbs_utils/rbs_utils_interfaces/srv/GetGraspPose.srv b/rbs_utils/rbs_utils_interfaces/srv/GetGraspPose.srv new file mode 100644 index 0000000..6d1ba6f --- /dev/null +++ b/rbs_utils/rbs_utils_interfaces/srv/GetGraspPose.srv @@ -0,0 +1,4 @@ +string model_name +--- +rbs_utils_interfaces/RelativeNamedPose[] grasp_pose +bool ok diff --git a/rbs_utils/rbs_utils_interfaces/srv/GetWorkspace.srv b/rbs_utils/rbs_utils_interfaces/srv/GetWorkspace.srv new file mode 100644 index 0000000..92eb3d5 --- /dev/null +++ b/rbs_utils/rbs_utils_interfaces/srv/GetWorkspace.srv @@ -0,0 +1,4 @@ +string type +--- +geometry_msgs/Point[] workspace +bool ok diff --git a/repos/sim.rbs.repos b/repos/sim.rbs.repos index 51e4065..5966b05 100644 --- a/repos/sim.rbs.repos +++ b/repos/sim.rbs.repos @@ -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