diff --git a/rbs_mill_assist/CMakeLists.txt b/rbs_mill_assist/CMakeLists.txt index 84deb0d..62423de 100644 --- a/rbs_mill_assist/CMakeLists.txt +++ b/rbs_mill_assist/CMakeLists.txt @@ -7,51 +7,13 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(std_srvs REQUIRED) -find_package(gz-cmake3 REQUIRED) -find_package(gz-plugin2 REQUIRED COMPONENTS register) -set(GZ_PLUGIN_VER ${gz-plugin2_VERSION_MAJOR}) -find_package(gz-common5 REQUIRED COMPONENTS profiler) -set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR}) - -# Harmonic -if("$ENV{GZ_VERSION}" STREQUAL "harmonic") - find_package(gz-sim8 REQUIRED) - set(GZ_SIM_VER ${gz-sim8_VERSION_MAJOR}) - message(STATUS "Compiling against Gazebo Harmonic") -# Default to Garden -else() - find_package(gz-sim7 REQUIRED) - set(GZ_SIM_VER ${gz-sim7_VERSION_MAJOR}) - message(STATUS "Compiling against Gazebo Garden") -endif() - -add_library(VacuumGripper - SHARED - src/VacuumGripper.cpp -) include_directories(include) -target_link_libraries(VacuumGripper - gz-sim${GZ_SIM_VER}::gz-sim${GZ_SIM_VER} -) - -ament_target_dependencies(VacuumGripper - rclcpp - std_srvs -) - -install( - TARGETS VacuumGripper - DESTINATION lib/${PROJECT_NAME} -) - - -install(DIRECTORY world urdf meshes launch config assets bt/xmls bt/config +install(DIRECTORY world urdf meshes moveit srdf launch config assets bt/xmls bt/config DESTINATION share/${PROJECT_NAME}) +add_subdirectory(src) add_subdirectory(scripts) add_subdirectory(bt) diff --git a/rbs_mill_assist/assets/bunker/meshes/Bunker_for_labels_60х40.STL b/rbs_mill_assist/assets/bunker/meshes/Bunker_for_labels_60х40.STL new file mode 100644 index 0000000..2ab7bf2 Binary files /dev/null and b/rbs_mill_assist/assets/bunker/meshes/Bunker_for_labels_60х40.STL differ diff --git a/rbs_mill_assist/assets/bunker/model.sdf b/rbs_mill_assist/assets/bunker/model.sdf index ede8a4e..54faf4f 100644 --- a/rbs_mill_assist/assets/bunker/model.sdf +++ b/rbs_mill_assist/assets/bunker/model.sdf @@ -6,41 +6,41 @@ - model://bunker/meshes/bunker.stl + model://bunker/meshes/Bunker_for_labels_60х40.STL - - - - - - - - - - - - - - - - - - + + 1 1 1 1 + 1 1 1 1 + 0.5 0.5 0.5 1 + 0 0 0 1 + - + - model://bunker/meshes/bunker.stl + model://bunker/meshes/Bunker_for_labels_60х40.STL - + + 1e6 + 1e3 + 0.1 + 0.001 + - + + 0.3 + - + + 0.61 + 0.47 + 0.0 + 0.0 + diff --git a/rbs_mill_assist/assets/conductor/meshes/conductor.stl b/rbs_mill_assist/assets/conductor/meshes/conductor.stl new file mode 100644 index 0000000..7696eb8 Binary files /dev/null and b/rbs_mill_assist/assets/conductor/meshes/conductor.stl differ diff --git a/rbs_mill_assist/assets/conductor/model.config b/rbs_mill_assist/assets/conductor/model.config new file mode 100644 index 0000000..4d85e18 --- /dev/null +++ b/rbs_mill_assist/assets/conductor/model.config @@ -0,0 +1,16 @@ + + + + conductor + 0.1 + model.sdf + + + Bill Finger + ur.narmak@gmail.com + + + + Empty + + diff --git a/rbs_mill_assist/assets/conductor/model.sdf b/rbs_mill_assist/assets/conductor/model.sdf new file mode 100644 index 0000000..f4b0287 --- /dev/null +++ b/rbs_mill_assist/assets/conductor/model.sdf @@ -0,0 +1,60 @@ + + + + + + + + + model://conductor/meshes/conductor.stl + + + + 0.8 0.8 0.8 1 + 0.3 0.3 0.3 1 + 0.2 0.2 0.2 1 + 0 0 0 1 + + + + + + + + + + + + + + + + + model://conductor/meshes/conductor.stl + + + + + + 1e6 + 1e3 + 0.1 + 0.001 + + + + 0.3 + + + + 0.61 + 0.47 + 0.0 + 0.0 + + + + + + + diff --git a/rbs_mill_assist/assets/laser/laser.material b/rbs_mill_assist/assets/laser/laser.material new file mode 100644 index 0000000..d380252 --- /dev/null +++ b/rbs_mill_assist/assets/laser/laser.material @@ -0,0 +1,11 @@ +material LaserMaterial +{ + technique + { + pass + { + ambient 0.5 0.5 0.5 1.0 + diffuse 0.5 0.5 0.5 1.0 + } + } +} diff --git a/rbs_mill_assist/assets/laser/meshes/laser.dae b/rbs_mill_assist/assets/laser/meshes/laser.dae new file mode 100644 index 0000000..b3f6ca0 --- /dev/null +++ b/rbs_mill_assist/assets/laser/meshes/laser.dae @@ -0,0 +1,207 @@ + + + + + Blender User + Blender 4.3.2 commit date:2024-12-16, commit time:21:10, hash:32f5fdce0a0a + + 2025-03-05T17:38:05 + 2025-03-05T17:38:05 + + Z_UP + + + + + + + 39.59775 + 1.777778 + 0.1 + 100 + + + + + + 0 + 0 + 10 + + + + + + + + 39.59775 + 1.777778 + 0.1 + 100 + + + + + + 0 + 0 + 10 + + + + + + + + + 1000 1000 1000 + 1 + 0 + 0 + + + + + 0 + 0 + 2097153 + 1 + 1 + 1 + 1000 + 75 + 0.15 + 0.04999995 + 40 + 0.1 + 1 + 0.1 + 0.1 + 1 + + + + + + + 1000 1000 1000 + 1 + 0 + 0 + + + + + 0 + 0 + 2097153 + 1 + 1 + 1 + 1000 + 75 + 0.15 + 0.04999995 + 40 + 0.1 + 1 + 0.1 + 0.1 + 1 + + + + + + + + + + + 0 0 0 1 + + + 0.1584888 0.3396751 0.8005451 1 + + + 1.5 + + + + + + + + + + + + + + + + + -0.09499996 0.004999995 0.03999996 -0.09499996 0.004999995 -0.03999996 -0.09499996 0.305 0.03999996 -0.09499996 0.305 -0.03999996 -0.175 0.004999995 -0.03999996 -0.175 0.305 -0.03999996 -0.175 0.004999995 0.03999996 -0.175 0.305 0.03999996 -0.225 0.004999995 -0.155 -0.225 0.004999995 0.155 0.225 0.004999995 0.155 0.225 0.004999995 -0.155 0.225 0 -0.155 0.225 0 0.155 -0.225 0 -0.155 -0.225 0 0.155 -0.225 0.455 -0.07249999 -0.175 0.455 -0.03999996 -0.09499996 0.455 -0.03999996 -0.175 0.455 0.03999996 -0.225 0.455 0.07249999 -0.09499996 0.455 0.03999996 0.175 0.455 0.07249999 0.175 0.455 -0.07249999 0.175 0.305 0.07249999 -0.225 0.305 0.07249999 0.175 0.305 -0.07249999 -0.225 0.305 -0.07249999 -0.175 0.605 -0.03999996 -0.175 0.605 0.03999996 -0.09499996 0.605 -0.03999996 -0.09499996 0.605 0.03999996 + + + + + + + + + + 1 0 0 0 0 -1 -1 0 0 0 0 1 0 1 0 0 1 0 0 -1 0 0 1 2.8656e-6 0 1 -2.29248e-6 0 1 2.29248e-6 0 -1 1.14624e-6 0 -1 -5.73121e-7 0 -1 -2.8656e-6 0 -1 2.8656e-6 + + + + + + + + + + + + + + +

0 0 1 0 2 0 2 0 1 0 3 0 1 1 4 1 3 1 3 1 4 1 5 1 4 2 6 2 5 2 5 2 6 2 7 2 6 3 0 3 7 3 7 3 0 3 2 3 4 4 8 4 6 4 6 4 8 4 9 4 6 4 9 4 0 4 0 5 9 5 10 5 0 4 10 4 1 4 1 4 10 4 11 4 1 4 11 4 4 4 4 4 11 4 8 4 11 0 10 0 12 0 12 0 10 0 13 0 8 1 11 1 14 1 14 1 11 1 12 1 9 2 8 2 15 2 15 2 8 2 14 2 10 3 9 3 13 3 13 3 9 3 15 3 15 6 14 6 13 6 13 6 14 6 12 6 16 7 17 7 18 7 17 4 16 4 19 4 19 4 16 4 20 4 19 4 20 4 21 4 21 8 20 8 22 8 21 4 22 4 18 4 18 4 22 4 23 4 18 9 23 9 16 9 2 10 24 10 25 10 24 6 2 6 26 6 26 6 2 6 3 6 26 11 3 11 27 11 27 12 3 12 5 12 27 6 5 6 25 6 25 6 5 6 7 6 25 13 7 13 2 13 23 0 22 0 26 0 26 0 22 0 24 0 16 1 23 1 27 1 27 1 23 1 26 1 20 2 16 2 25 2 25 2 16 2 27 2 22 3 20 3 24 3 24 3 20 3 25 3 28 4 29 4 30 4 30 4 29 4 31 4 21 3 31 3 19 3 19 3 31 3 29 3 19 2 29 2 17 2 17 2 29 2 28 2 17 1 28 1 18 1 18 1 28 1 30 1 18 0 30 0 21 0 21 0 30 0 31 0

+ + + + + + + + -0.2908646 -0.7711008 0.5663933 4.076245 0.9551712 -0.1998833 0.2183913 1.005454 -0.05518912 0.6045247 0.7946723 5.903862 0 0 0 1 + + + + 0.6859207 -0.3240135 0.6515582 7.358891 0.7276763 0.3054209 -0.6141704 -6.925791 -4.01133e-9 0.8953956 0.4452714 4.958309 0 0 0 1 + + + + 1 0 0 0 0 7.54979e-8 -1 0 0 1 7.54979e-8 0 0 0 0 1 + + + + + + + + + + 0.6859207 -0.3240135 0.6515582 7.358891 0.7276763 0.3054208 -0.6141704 -6.925791 0 0.8953956 0.4452714 4.958309 0 0 0 1 + + + + -0.2908646 -0.7711008 0.5663932 4.076245 0.9551712 -0.1998834 0.2183912 1.005454 -0.05518906 0.6045247 0.7946723 5.903862 0 0 0 1 + + + + + + + + \ No newline at end of file diff --git a/rbs_mill_assist/assets/laser/meshes/laser.mtl b/rbs_mill_assist/assets/laser/meshes/laser.mtl new file mode 100644 index 0000000..24c0d6a --- /dev/null +++ b/rbs_mill_assist/assets/laser/meshes/laser.mtl @@ -0,0 +1,2 @@ +# Blender 4.3.2 MTL File: 'None' +# www.blender.org diff --git a/rbs_mill_assist/assets/laser/meshes/laser.obj b/rbs_mill_assist/assets/laser/meshes/laser.obj new file mode 100644 index 0000000..e3c99c5 --- /dev/null +++ b/rbs_mill_assist/assets/laser/meshes/laser.obj @@ -0,0 +1,103 @@ +# Blender 4.3.2 +# www.blender.org +mtllib laser.mtl +o laser +v -0.095000 0.005000 0.040000 +v -0.095000 0.005000 -0.040000 +v -0.095000 0.305000 0.040000 +v -0.095000 0.305000 -0.040000 +v -0.175000 0.005000 -0.040000 +v -0.175000 0.305000 -0.040000 +v -0.175000 0.005000 0.040000 +v -0.175000 0.305000 0.040000 +v -0.225000 0.005000 -0.155000 +v -0.225000 0.005000 0.155000 +v 0.225000 0.005000 0.155000 +v 0.225000 0.005000 -0.155000 +v 0.225000 0.000000 -0.155000 +v 0.225000 -0.000000 0.155000 +v -0.225000 0.000000 -0.155000 +v -0.225000 -0.000000 0.155000 +v -0.225000 0.455000 -0.072500 +v -0.175000 0.455000 -0.040000 +v -0.095000 0.455000 -0.040000 +v -0.175000 0.455000 0.040000 +v -0.225000 0.455000 0.072500 +v -0.095000 0.455000 0.040000 +v 0.175000 0.455000 0.072500 +v 0.175000 0.455000 -0.072500 +v 0.175000 0.305000 0.072500 +v -0.225000 0.305000 0.072500 +v 0.175000 0.305000 -0.072500 +v -0.225000 0.305000 -0.072500 +v -0.175000 0.605000 -0.040000 +v -0.175000 0.605000 0.040000 +v -0.095000 0.605000 -0.040000 +v -0.095000 0.605000 0.040000 +vn 1.0000 -0.0000 -0.0000 +vn -0.0000 -0.0000 -1.0000 +vn -1.0000 -0.0000 -0.0000 +vn -0.0000 -0.0000 1.0000 +vn -0.0000 1.0000 -0.0000 +vn -0.0000 -1.0000 -0.0000 +s 0 +f 1//1 2//1 3//1 +f 3//1 2//1 4//1 +f 2//2 5//2 4//2 +f 4//2 5//2 6//2 +f 5//3 7//3 6//3 +f 6//3 7//3 8//3 +f 7//4 1//4 8//4 +f 8//4 1//4 3//4 +f 5//5 9//5 7//5 +f 7//5 9//5 10//5 +f 7//5 10//5 1//5 +f 1//5 10//5 11//5 +f 1//5 11//5 2//5 +f 2//5 11//5 12//5 +f 2//5 12//5 5//5 +f 5//5 12//5 9//5 +f 12//1 11//1 13//1 +f 13//1 11//1 14//1 +f 9//2 12//2 15//2 +f 15//2 12//2 13//2 +f 10//3 9//3 16//3 +f 16//3 9//3 15//3 +f 11//4 10//4 14//4 +f 14//4 10//4 16//4 +f 16//6 15//6 14//6 +f 14//6 15//6 13//6 +f 17//5 18//5 19//5 +f 18//5 17//5 20//5 +f 20//5 17//5 21//5 +f 20//5 21//5 22//5 +f 22//5 21//5 23//5 +f 22//5 23//5 19//5 +f 19//5 23//5 24//5 +f 19//5 24//5 17//5 +f 3//6 25//6 26//6 +f 25//6 3//6 27//6 +f 27//6 3//6 4//6 +f 27//6 4//6 28//6 +f 28//6 4//6 6//6 +f 28//6 6//6 26//6 +f 26//6 6//6 8//6 +f 26//6 8//6 3//6 +f 24//1 23//1 27//1 +f 27//1 23//1 25//1 +f 17//2 24//2 28//2 +f 28//2 24//2 27//2 +f 21//3 17//3 26//3 +f 26//3 17//3 28//3 +f 23//4 21//4 25//4 +f 25//4 21//4 26//4 +f 29//5 30//5 31//5 +f 31//5 30//5 32//5 +f 22//4 32//4 20//4 +f 20//4 32//4 30//4 +f 20//3 30//3 18//3 +f 18//3 30//3 29//3 +f 18//2 29//2 19//2 +f 19//2 29//2 31//2 +f 19//1 31//1 22//1 +f 22//1 31//1 32//1 diff --git a/rbs_mill_assist/assets/laser/meshes/laser.stl b/rbs_mill_assist/assets/laser/meshes/laser.stl index a15df0e..383c7ab 100644 Binary files a/rbs_mill_assist/assets/laser/meshes/laser.stl and b/rbs_mill_assist/assets/laser/meshes/laser.stl differ diff --git a/rbs_mill_assist/assets/laser/model.sdf b/rbs_mill_assist/assets/laser/model.sdf index 0713e03..2cdf5de 100644 --- a/rbs_mill_assist/assets/laser/model.sdf +++ b/rbs_mill_assist/assets/laser/model.sdf @@ -7,32 +7,32 @@ - model://laser/meshes/laser.stl + model://laser/meshes/laser.obj - - - - - - - - - - - - - - - - - - + + 1 1 1 1 + 1 1 1 1 + 0.5 0.5 0.5 1 + 0 0 0 1 + + + + + + + + + + + + + - model://laser/meshes/laser.stl + model://laser/meshes/laser.obj diff --git a/rbs_mill_assist/assets/shildik/meshes/shildik_sh.dae b/rbs_mill_assist/assets/shildik/meshes/shildik_sh.dae index 3106369..e7108de 100644 --- a/rbs_mill_assist/assets/shildik/meshes/shildik_sh.dae +++ b/rbs_mill_assist/assets/shildik/meshes/shildik_sh.dae @@ -3,13 +3,66 @@ Blender User - Blender 4.1.1 commit date:2024-04-15, commit time:15:11, hash:e1743a0317bc + Blender 4.3.2 commit date:2024-12-16, commit time:21:10, hash:32f5fdce0a0a - 2025-02-12T10:55:51 - 2025-02-12T10:55:51 + 2025-03-03T12:34:37 + 2025-03-03T12:34:37 Z_UP + + + + + + 39.59775 + 1.777778 + 0.1 + 100 + + + + + + 0 + 0 + 10 + + + + + + + + + 1000 1000 1000 + 1 + 0 + 0 + + + + + 0 + 0 + 2097153 + 1 + 1 + 1 + 1000 + 75 + 0.15 + 0.04999995 + 40 + 0.1 + 1 + 0.1 + 0.1 + 1 + + + + @@ -39,7 +92,7 @@ - -0.02999997 -0.01999998 0 -0.02999997 -0.01999998 5e-4 -0.02999997 0.01999998 0 -0.02999997 0.01999998 5e-4 0.02999997 -0.01999998 0 0.02999997 -0.01999998 5e-4 0.02999997 0.01999998 0 0.02999997 0.01999998 5e-4 -0.02999997 0 0 -0.02999997 0 5e-4 0 0.01999998 0 0 0.01999998 5e-4 0.02999997 0 0 0.02999997 0 5e-4 0 -0.01999998 0 0 -0.01999998 5e-4 0 0 5e-4 0 0 0 + -0.02999997 -0.01999998 -2.5e-4 -0.02999997 -0.01999998 2.5e-4 -0.02999997 0.01999998 -2.5e-4 -0.02999997 0.01999998 2.5e-4 0.02999997 -0.01999998 -2.5e-4 0.02999997 -0.01999998 2.5e-4 0.02999997 0.01999998 -2.5e-4 0.02999997 0.01999998 2.5e-4 -0.02999997 0 -2.5e-4 -0.02999997 0 2.5e-4 0 0.01999998 -2.5e-4 0 0.01999998 2.5e-4 0.02999997 0 -2.5e-4 0.02999997 0 2.5e-4 0 -0.01999998 -2.5e-4 0 -0.01999998 2.5e-4 0 0 2.5e-4 0 0 -2.5e-4 @@ -87,12 +140,20 @@ - + + + 0.6859207 -0.3240135 0.6515582 7.358891 0.7276763 0.3054208 -0.6141704 -6.925791 0 0.8953956 0.4452714 4.958309 0 0 0 1 + + + + -0.2908646 -0.7711008 0.5663932 4.076245 0.9551712 -0.1998834 0.2183912 1.005454 -0.05518906 0.6045247 0.7946723 5.903862 0 0 0 1 + + diff --git a/rbs_mill_assist/assets/shildik/model.sdf b/rbs_mill_assist/assets/shildik/model.sdf index 1674d34..1761925 100644 --- a/rbs_mill_assist/assets/shildik/model.sdf +++ b/rbs_mill_assist/assets/shildik/model.sdf @@ -31,7 +31,7 @@ - 0.06 0.04 0.001 + 0.06 0.04 0.0005 diff --git a/rbs_mill_assist/bt/CMakeLists.txt b/rbs_mill_assist/bt/CMakeLists.txt index 70f2b16..1d6a06b 100644 --- a/rbs_mill_assist/bt/CMakeLists.txt +++ b/rbs_mill_assist/bt/CMakeLists.txt @@ -2,8 +2,9 @@ find_package(behaviortree_ros2 REQUIRED) find_package(behaviortree_cpp REQUIRED) find_package(rbs_skill_interfaces REQUIRED) +find_package(rbs_utils_interfaces REQUIRED) find_package(geometry_msgs REQUIRED) -# find_package(std_srvs REQUIRED) +find_package(std_srvs REQUIRED) # Behaviortree interfaces set(dependencies @@ -11,6 +12,7 @@ set(dependencies rbs_skill_interfaces geometry_msgs behaviortree_ros2 + rbs_utils_interfaces std_srvs ) @@ -20,6 +22,15 @@ list(APPEND plugin_libs vacuum_gripper_toggle) add_library(get_grasp_place_pose SHARED plugins/get_grasp_place_pose.cpp) list(APPEND plugin_libs get_grasp_place_pose) +add_library(get_named_pose SHARED plugins/get_named_pose.cpp) +list(APPEND plugin_libs get_named_pose) + +add_library(is_in_pose SHARED plugins/is_in_pose.cpp) +list(APPEND plugin_libs is_in_pose) + +add_library(twist_command_with_condition SHARED plugins/twist_cmd.cpp) +list(APPEND plugin_libs twist_command_with_condition) + foreach(bt_plugin ${plugin_libs}) ament_target_dependencies(${bt_plugin} ${dependencies}) target_compile_definitions(${bt_plugin} PRIVATE BT_PLUGIN_EXPORT) diff --git a/rbs_mill_assist/bt/plugins/get_grasp_place_pose.cpp b/rbs_mill_assist/bt/plugins/get_grasp_place_pose.cpp index 7eeb671..f8a2a95 100644 --- a/rbs_mill_assist/bt/plugins/get_grasp_place_pose.cpp +++ b/rbs_mill_assist/bt/plugins/get_grasp_place_pose.cpp @@ -34,11 +34,12 @@ public: } bool setRequest(Request::SharedPtr &request) override { - RCLCPP_INFO(this->logger(), "Starting send request for: [%s]", - this->service_name_.c_str()); + RCLCPP_INFO(this->logger(), "[%s] Starting send request for: [%s]", + name().c_str(), this->service_name_.c_str()); if (!getInput("action_name", request->action_name)) { RCLCPP_ERROR(this->node_.lock()->get_logger(), - "Failed to get object_name from input port"); + "[%s] Failed to get object_name from input port", + name().c_str()); return false; } return true; @@ -47,25 +48,25 @@ public: NodeStatus onResponseReceived(const Response::SharedPtr &response) override { if (!response->ok) { RCLCPP_ERROR(this->node_.lock()->get_logger(), - "Response indicates failure."); + "[%s] Response indicates failure.", name().c_str()); return NodeStatus::FAILURE; } RCLCPP_INFO(this->node_.lock()->get_logger(), - "Response received successfully."); + "[%s] Response received successfully.", name().c_str()); auto logPose = [this](const std::string &pose_name, const geometry_msgs::msg::Pose &pose) { RCLCPP_INFO(this->node_.lock()->get_logger(), - "%s:\n" + "[%s] %s:\n" " Position: x = %.4f, y = %.4f, z = %.4f\n" " Orientation: x = %.4f, y = %.4f, z = %.4f, w = %.4f", - pose_name.c_str(), pose.position.x, pose.position.y, - pose.position.z, pose.orientation.x, pose.orientation.y, - pose.orientation.z, pose.orientation.w); + name().c_str(), pose_name.c_str(), pose.position.x, + pose.position.y, pose.position.z, pose.orientation.x, + pose.orientation.y, pose.orientation.z, pose.orientation.w); }; if (!response->grasp.empty()) { - RCLCPP_INFO(this->logger(), "Got Pick Response"); + RCLCPP_INFO(this->logger(), "[%s] Got Pick Response", name().c_str()); std::vector poses = {"Pregrasp Pose", "Grasp Pose", "Postgrasp Pose"}; @@ -87,7 +88,7 @@ public: return NodeStatus::SUCCESS; } if (!response->place.empty()) { - RCLCPP_INFO(this->logger(), "Got Place Response"); + RCLCPP_INFO(this->logger(), "[%s] Got Place Response", name().c_str()); std::vector poses = {"Preplace Pose", "Place Pose", "Postplace Pose"}; @@ -109,7 +110,7 @@ public: return NodeStatus::SUCCESS; } - RCLCPP_FATAL(this->logger(), "Could not response grasp pose"); + RCLCPP_FATAL(this->logger(), "[%s] Could not response grasp pose", name().c_str()); return NodeStatus::FAILURE; } // virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {} diff --git a/rbs_mill_assist/bt/plugins/get_named_pose.cpp b/rbs_mill_assist/bt/plugins/get_named_pose.cpp new file mode 100644 index 0000000..e14ad5d --- /dev/null +++ b/rbs_mill_assist/bt/plugins/get_named_pose.cpp @@ -0,0 +1,65 @@ +#include "behaviortree_ros2/bt_service_node.hpp" + +#include "behaviortree_ros2/plugins.hpp" +#include +// #include +#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp" +#include "rbs_utils_interfaces/srv/get_named_pose.hpp" +#include +#include +#include +#include +#include +#include + +using GetNamedPoseService = rbs_utils_interfaces::srv::GetNamedPose; +using namespace BT; + +class GetNamedPose : public RosServiceNode { +public: + GetNamedPose(const std::string &name, const NodeConfig &conf, + const RosNodeParams ¶ms) + : RosServiceNode(name, conf, params) { + + RCLCPP_INFO(this->logger(), "Starting GetGraspPose"); + } + + static PortsList providedPorts() { + return providedBasicPorts( + {InputPort("pose_name"), + OutputPort>("output_pose")}); + } + + bool setRequest(Request::SharedPtr &request) override { + RCLCPP_INFO(this->logger(), "[%s] Starting send request for: [%s]", + name().c_str(), this->service_name_.c_str()); + if (!getInput("pose_name", request->pose_name)) { + RCLCPP_ERROR(this->node_.lock()->get_logger(), + "[%s] Failed to get pose_name from input port", + name().c_str()); + return false; + } + return true; + } + + NodeStatus onResponseReceived(const Response::SharedPtr &response) override { + if (!response->ok) { + RCLCPP_ERROR(this->logger(), "[%s] Response indicates failure.", + name().c_str()); + return NodeStatus::FAILURE; + } + + RCLCPP_INFO(this->logger(), + "[%s] Response received successfully with pose name [%s]", + name().c_str(), response->named_pose.name.c_str()); + + auto pose = std::make_shared(); + *pose = response->named_pose.pose; + setOutput("output_pose", pose); + + return NodeStatus::SUCCESS; + } + // virtual NodeStatus onFailure(ServiceNodeErrorCode error) override {} +}; + +CreateRosNodePlugin(GetNamedPose, "GetNamedPose"); diff --git a/rbs_mill_assist/bt/plugins/is_in_pose.cpp b/rbs_mill_assist/bt/plugins/is_in_pose.cpp new file mode 100644 index 0000000..dda3714 --- /dev/null +++ b/rbs_mill_assist/bt/plugins/is_in_pose.cpp @@ -0,0 +1,48 @@ +#include "behaviortree_ros2/bt_topic_sub_node.hpp" + +#include "behaviortree_ros2/plugins.hpp" +#include +// #include +#include "rbs_skill_interfaces/srv/get_pick_place_poses.hpp" +#include "rbs_utils_interfaces/srv/get_named_pose.hpp" +#include +#include +#include +#include +#include +#include + +using MSG = geometry_msgs::msg::Pose; +using namespace BT; + +class IsInPose : public RosTopicSubNode { +public: + IsInPose(const std::string &name, const NodeConfig &conf, + const RosNodeParams ¶ms) + : RosTopicSubNode(name, conf, params) { + + RCLCPP_INFO(this->logger(), "Starting IsInPose"); + } + // + // static PortsList providedPorts() { + // return {}; + // } + + NodeStatus onTick(const std::shared_ptr &t_last_msg) override { + if (t_last_msg) { + RCLCPP_INFO(this->logger(), "[%s] Got message Pose", name().c_str()); + m_last_msg = t_last_msg; + return NodeStatus::SUCCESS; + } + RCLCPP_INFO(this->logger(), "[%s] NOT message received", name().c_str()); + return NodeStatus::SUCCESS; + } + bool latchLastMessage() const override { + return true; + } + +private: + MSG::SharedPtr m_last_msg; +}; + +CreateRosNodePlugin(IsInPose, "IsInPose"); diff --git a/rbs_mill_assist/bt/plugins/twist_cmd.cpp b/rbs_mill_assist/bt/plugins/twist_cmd.cpp new file mode 100644 index 0000000..7688e56 --- /dev/null +++ b/rbs_mill_assist/bt/plugins/twist_cmd.cpp @@ -0,0 +1,85 @@ +#include "behaviortree_ros2/bt_action_node.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "rbs_skill_interfaces/action/twist_control_with_condition.hpp" +#include +#include +#include +#include + +using namespace BT; +using TwistCmdWithCondAction = + rbs_skill_interfaces::action::TwistControlWithCondition; + +class TwistCmdWithCondClient : public RosActionNode { +public: + TwistCmdWithCondClient(const std::string &name, const NodeConfig &conf, + const RosNodeParams ¶ms) + : RosActionNode(name, conf, params) { + + RCLCPP_INFO(this->logger(), "Starting MoveToPose"); + } + + static BT::PortsList providedPorts() { + return providedBasicPorts({BT::InputPort("robot_name"), + BT::InputPort>("twist"), + // BT::InputPort("condition_topic") + }); + } + + bool setGoal(RosActionNode::Goal &goal) override { + RCLCPP_INFO(this->logger(), "[%s] Starting send goal [%s]", name().c_str(), + this->action_name_.c_str()); + + if (!getInput("robot_name", goal.robot_name)) { + RCLCPP_FATAL(this->logger(), "[%s] Could not get robot name", + name().c_str()); + return false; + } + + if (!getInput("twist", twist_cmd_vec)) { + RCLCPP_FATAL(this->logger(), "[%s] Could not get twist command", + name().c_str()); + return false; + } + if (!twist_cmd_vec.empty() && twist_cmd_vec.size() == 6) { + goal.twist_cmd.linear.x = twist_cmd_vec[0]; + goal.twist_cmd.linear.y = twist_cmd_vec[1]; + goal.twist_cmd.linear.z = twist_cmd_vec[2]; + goal.twist_cmd.angular.x = twist_cmd_vec[3]; + goal.twist_cmd.angular.y = twist_cmd_vec[4]; + goal.twist_cmd.angular.z = twist_cmd_vec[5]; + } else { + RCLCPP_FATAL(this->logger(), + "[%s] Twist command is empty or should be 1x6 [x;y;z;r;p;y]", + name().c_str()); + return false; + } + + // if (!getInput("condition_topic", goal.condition_topic)) { + // RCLCPP_FATAL(this->logger(), "[%s] Could not get condition_topic", + // name().c_str()); + // return false; + // } else { + // RCLCPP_INFO(this->logger(), "[%s] Using condition_topic [%s]", + // name().c_str(), goal.condition_topic.c_str()); + // } + + return true; + } + + NodeStatus onResultReceived(const WrappedResult &wr) override { + + RCLCPP_INFO(this->logger(), "[%s] Starting get response %s with status %b", + name().c_str(), this->action_name_.c_str(), wr.result->success); + if (!wr.result->success) { + return NodeStatus::FAILURE; + } + return NodeStatus::SUCCESS; + } + +private: + // std::shared_ptr m_target_pose; + std::vector twist_cmd_vec; +}; + +CreateRosNodePlugin(TwistCmdWithCondClient, "TwistCmdWithCond"); diff --git a/rbs_mill_assist/bt/plugins/vacuum_gripper_toggle.cpp b/rbs_mill_assist/bt/plugins/vacuum_gripper_toggle.cpp index a7a2fb1..5bb5c04 100644 --- a/rbs_mill_assist/bt/plugins/vacuum_gripper_toggle.cpp +++ b/rbs_mill_assist/bt/plugins/vacuum_gripper_toggle.cpp @@ -24,10 +24,10 @@ public: } bool setRequest(Request::SharedPtr &request) override { - RCLCPP_INFO(this->logger(), "Starting send request"); + RCLCPP_INFO(this->logger(), "[%s] Starting send request", name().c_str()); if (!getInput("enable", request->data)) { - RCLCPP_ERROR(this->node_.lock()->get_logger(), - "Failed to get sending data from input port"); + RCLCPP_ERROR(this->logger(), + "[%s] Failed to get sending data from input port", name().c_str()); return false; } return true; @@ -35,7 +35,7 @@ public: NodeStatus onResponseReceived(const Response::SharedPtr &response) override { if (!response->success) { - RCLCPP_ERROR(this->logger(), "Response indicates failure."); + RCLCPP_ERROR(this->logger(), "[%s] Response indicates failure.", name().c_str()); return NodeStatus::FAILURE; } diff --git a/rbs_mill_assist/bt/xmls/Grasp.xml b/rbs_mill_assist/bt/xmls/Grasp.xml index 4d1bde0..56ebcf1 100644 --- a/rbs_mill_assist/bt/xmls/Grasp.xml +++ b/rbs_mill_assist/bt/xmls/Grasp.xml @@ -3,19 +3,35 @@ -