diff --git a/rbs_mill_assist/assets/laser/model.sdf b/rbs_mill_assist/assets/laser/model.sdf
index 0713e03..973ebc7 100644
--- a/rbs_mill_assist/assets/laser/model.sdf
+++ b/rbs_mill_assist/assets/laser/model.sdf
@@ -10,24 +10,24 @@
model://laser/meshes/laser.stl
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
-
+
+ 1 1 1 1
+ 1 1 1 1
+ 0.5 0.5 0.5 1
+ 0 0 0 1
+
+
+
+
+
+
+
+
+
+
+
+
+
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..c8c018f 100644
--- a/rbs_mill_assist/bt/CMakeLists.txt
+++ b/rbs_mill_assist/bt/CMakeLists.txt
@@ -2,6 +2,7 @@
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)
@@ -11,6 +12,7 @@ set(dependencies
rbs_skill_interfaces
geometry_msgs
behaviortree_ros2
+ rbs_utils_interfaces
std_srvs
)
@@ -20,6 +22,9 @@ 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)
+
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_named_pose.cpp b/rbs_mill_assist/bt/plugins/get_named_pose.cpp
new file mode 100644
index 0000000..3b50b25
--- /dev/null
+++ b/rbs_mill_assist/bt/plugins/get_named_pose.cpp
@@ -0,0 +1,63 @@
+#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(), "Starting send request for: [%s]",
+ this->service_name_.c_str());
+ if (!getInput("pose_name", request->pose_name)) {
+ RCLCPP_ERROR(this->node_.lock()->get_logger(),
+ "Failed to get pose_name from input port");
+ return false;
+ }
+ return true;
+ }
+
+ NodeStatus onResponseReceived(const Response::SharedPtr &response) override {
+ if (!response->ok) {
+ RCLCPP_ERROR(this->logger(), "Response indicates failure.");
+ return NodeStatus::FAILURE;
+ }
+
+ RCLCPP_INFO(this->logger(),
+ "Response received successfully with pose name [%s]",
+ 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/xmls/FromGraver.xml b/rbs_mill_assist/bt/xmls/FromGraver.xml
new file mode 100644
index 0000000..7faccd8
--- /dev/null
+++ b/rbs_mill_assist/bt/xmls/FromGraver.xml
@@ -0,0 +1,9 @@
+
+
+
+
+
+
+
+
+
diff --git a/rbs_mill_assist/bt/xmls/Grasp.xml b/rbs_mill_assist/bt/xmls/Grasp.xml
index 4d1bde0..5a8fb9e 100644
--- a/rbs_mill_assist/bt/xmls/Grasp.xml
+++ b/rbs_mill_assist/bt/xmls/Grasp.xml
@@ -2,16 +2,12 @@
+
-
-
-
-
-
-
-
-
-
+
+
+
+
diff --git a/rbs_mill_assist/bt/xmls/MainTree.xml b/rbs_mill_assist/bt/xmls/MainTree.xml
index 2133fa1..cf46ffa 100644
--- a/rbs_mill_assist/bt/xmls/MainTree.xml
+++ b/rbs_mill_assist/bt/xmls/MainTree.xml
@@ -2,8 +2,19 @@
-
-
+
+
+
+
+
+
+
+
+
+
+
diff --git a/rbs_mill_assist/bt/xmls/Place.xml b/rbs_mill_assist/bt/xmls/Place.xml
index b7be311..6d420e7 100644
--- a/rbs_mill_assist/bt/xmls/Place.xml
+++ b/rbs_mill_assist/bt/xmls/Place.xml
@@ -2,16 +2,12 @@
+
-
-
-
-
-
-
-
-
-
+
+
+
+
diff --git a/rbs_mill_assist/bt/xmls/ToGraver.xml b/rbs_mill_assist/bt/xmls/ToGraver.xml
new file mode 100644
index 0000000..a57816b
--- /dev/null
+++ b/rbs_mill_assist/bt/xmls/ToGraver.xml
@@ -0,0 +1,9 @@
+
+
+
+
+
+
+
+
+
diff --git a/rbs_mill_assist/config/grasping.yaml b/rbs_mill_assist/config/grasping.yaml
index 02a27ae..98fbd52 100644
--- a/rbs_mill_assist/config/grasping.yaml
+++ b/rbs_mill_assist/config/grasping.yaml
@@ -1,60 +1,123 @@
-pregrasp_pose:
- position:
- x: 0.10395
- y: -0.28
- z: 0.1
- orientation:
- x: 0.0
- y: 1.0
- z: 0.0
- w: 0.0
-grasp_pose:
- position:
- x: 0.10395
- y: -0.28
- z: 0.004
- orientation:
- x: 0.0
- y: 1.0
- z: 0.0
- w: 0.04
-postgrasp_pose:
- position:
- x: 0.10395
- y: -0.28
- z: 0.1
- orientation:
- x: 0.0
- y: 1.0
- z: 0.0
- w: 0.0
-preplace_pose:
- position:
- x: 0.360
- y: -0.06
- z: 0.1
- orientation:
- x: -0.707
- y: 0.707
- z: 0.0
- w: 0.0
-place_pose:
- position:
- x: 0.360
- y: -0.06
- z: 0.05
- orientation:
- x: -0.707
- y: 0.707
- z: 0.0
- w: 0.0
-postplace_pose:
- position:
- x: 0.360
- y: -0.06
- z: 0.1
- orientation:
- x: -0.707
- y: 0.707
- z: 0.0
- w: 0.0
+to_graver:
+ pregrasp_pose:
+ position:
+ x: 0.10395
+ y: -0.28
+ z: 0.1
+ orientation:
+ x: 0.0
+ y: 1.0
+ z: 0.0
+ w: 0.0
+ grasp_pose:
+ position:
+ x: 0.10395
+ y: -0.28
+ z: 0.001
+ orientation:
+ x: 0.0
+ y: 1.0
+ z: 0.0
+ w: 0.0
+ postgrasp_pose:
+ position:
+ x: 0.10395
+ y: -0.28
+ z: 0.1
+ orientation:
+ x: 0.0
+ y: 1.0
+ z: 0.0
+ w: 0.0
+ preplace_pose:
+ position:
+ x: 0.330
+ y: -0.06
+ z: 0.1
+ orientation:
+ x: -0.707
+ y: 0.707
+ z: 0.0
+ w: 0.0
+ place_pose:
+ position:
+ x: 0.330
+ y: -0.06
+ z: 0.01
+ orientation:
+ x: -0.707
+ y: 0.707
+ z: 0.0
+ w: 0.0
+ postplace_pose:
+ position:
+ x: 0.330
+ y: -0.06
+ z: 0.1
+ orientation:
+ x: -0.707
+ y: 0.707
+ z: 0.0
+ w: 0.0
+
+from_graver:
+ pregrasp_pose:
+ position:
+ x: 0.330
+ y: -0.06
+ z: 0.1
+ orientation:
+ x: -0.707
+ y: 0.707
+ z: 0.0
+ w: 0.0
+ grasp_pose:
+ position:
+ x: 0.330
+ y: -0.06
+ z: 0.0
+ orientation:
+ x: -0.707
+ y: 0.707
+ z: 0.0
+ w: 0.0
+ postgrasp_pose:
+ position:
+ x: 0.330
+ y: -0.06
+ z: 0.1
+ orientation:
+ x: -0.707
+ y: 0.707
+ z: 0.0
+ w: 0.0
+ preplace_pose:
+ position:
+ x: 0.10395
+ y: 0.28
+ z: 0.1
+ orientation:
+ x: -1.000
+ y: 0.0
+ z: 0.0
+ w: 0.0
+ place_pose:
+ position:
+ x: 0.10395
+ y: 0.28
+ z: 0.000
+ orientation:
+ x: -1.000
+ y: 0.0
+ z: 0.0
+ w: 0.0
+ postplace_pose:
+ position:
+ x: 0.10395
+ y: 0.28
+ z: 0.1
+ orientation:
+ x: -1.000
+ y: 0.0
+ z: 0.0
+ w: 0.0
diff --git a/rbs_mill_assist/config/key_poses.yaml b/rbs_mill_assist/config/key_poses.yaml
new file mode 100644
index 0000000..a5c6331
--- /dev/null
+++ b/rbs_mill_assist/config/key_poses.yaml
@@ -0,0 +1,10 @@
+waiting:
+ position:
+ x: 0.2116573005914688
+ y: -0.10102221369743347
+ z: 0.1460307389497757
+ orientation:
+ x: 0.0
+ y: 1.0
+ z: 0.0
+ w: 0.0
diff --git a/rbs_mill_assist/launch/simulation.launch.py b/rbs_mill_assist/launch/simulation.launch.py
index f125236..7ddf446 100644
--- a/rbs_mill_assist/launch/simulation.launch.py
+++ b/rbs_mill_assist/launch/simulation.launch.py
@@ -143,7 +143,7 @@ def launch_setup(context, *args, **kwargs):
PythonLaunchDescriptionSource(
[os.path.join(get_package_share_directory('ros_gz_sim'),
'launch', 'gz_sim.launch.py')]),
- launch_arguments=[('gz_args', [' -r -v 4 ', gazebo_world])],
+ launch_arguments=[('gz_args', [' -r ', gazebo_world])],
)
gazebo_spawn_robot = Node(
@@ -170,13 +170,19 @@ def launch_setup(context, *args, **kwargs):
executable="grasping_service.py"
)
+ get_named_pose_service = Node(
+ package="rbs_mill_assist",
+ executable="get_key_pose_frame.py"
+ )
+
nodes_to_start = [
rbs_robot_setup,
rviz_node,
gazebo,
gazebo_spawn_robot,
gz_bridge,
- grasping_service
+ grasping_service,
+ get_named_pose_service
]
return nodes_to_start
diff --git a/rbs_mill_assist/scripts/CMakeLists.txt b/rbs_mill_assist/scripts/CMakeLists.txt
index fa24557..73a80a0 100644
--- a/rbs_mill_assist/scripts/CMakeLists.txt
+++ b/rbs_mill_assist/scripts/CMakeLists.txt
@@ -1,4 +1,5 @@
install(PROGRAMS
grasping_service.py
+ get_key_pose_frame.py
DESTINATION lib/${PROJECT_NAME}
)
diff --git a/rbs_mill_assist/scripts/get_key_pose_frame.py b/rbs_mill_assist/scripts/get_key_pose_frame.py
new file mode 100755
index 0000000..790fee0
--- /dev/null
+++ b/rbs_mill_assist/scripts/get_key_pose_frame.py
@@ -0,0 +1,72 @@
+#!/usr/bin/env python3
+
+import os
+from typing import Dict
+
+import rclpy
+import yaml
+from ament_index_python.packages import get_package_share_directory
+from geometry_msgs.msg import Pose
+from rbs_utils_interfaces.srv import GetNamedPose
+from rclpy.node import Node
+from rclpy.service import Service
+
+
+class GetNamedPoseService(Node):
+ def __init__(self) -> None:
+ super().__init__("get_named_pose_service")
+ self.srv: Service = self.create_service(
+ GetNamedPose, "get_named_pose", self.get_named_pose
+ )
+ yaml_file_path: str = os.path.join(
+ get_package_share_directory("rbs_mill_assist"), "config", "key_poses.yaml"
+ )
+ with open(yaml_file_path, "r", encoding="utf-8") as file:
+ self.key_poses: Dict = yaml.safe_load(file)
+
+ def create_pose(self, pose_data: Dict) -> Pose:
+ """
+ Helper function to create a Pose from the given pose data.
+ """
+ pose = Pose()
+ if pose_data:
+ pose.position.x = pose_data.get("position", {}).get("x", 0.0)
+ pose.position.y = pose_data.get("position", {}).get("y", 0.0)
+ pose.position.z = pose_data.get("position", {}).get("z", 0.0)
+ pose.orientation.x = pose_data.get("orientation", {}).get("x", 0.0)
+ pose.orientation.y = pose_data.get("orientation", {}).get("y", 0.0)
+ pose.orientation.z = pose_data.get("orientation", {}).get("z", 0.0)
+ pose.orientation.w = pose_data.get("orientation", {}).get("w", 1.0)
+ return pose
+
+ def get_named_pose(
+ self, request: GetNamedPose.Request, response: GetNamedPose.Response
+ ) -> GetNamedPose.Response:
+
+ key_pose = self.key_poses.get(request.pose_name, None)
+ if key_pose is None:
+ response.ok = False
+ return response
+
+ response.named_pose.name = request.pose_name
+ response.named_pose.pose = self.create_pose(key_pose)
+ response.ok = True
+
+ return response
+
+
+def main():
+ rclpy.init()
+ executor = rclpy.executors.SingleThreadedExecutor()
+ # executor = rclpy.executors.MultiThreadedExecutor() # can't be used
+ i_node = GetNamedPoseService()
+ executor.add_node(i_node)
+ try:
+ executor.spin()
+ except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException):
+ i_node.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/rbs_mill_assist/scripts/grasping_service.py b/rbs_mill_assist/scripts/grasping_service.py
index bbec6a1..86b75a9 100755
--- a/rbs_mill_assist/scripts/grasping_service.py
+++ b/rbs_mill_assist/scripts/grasping_service.py
@@ -42,10 +42,11 @@ class GetGraspPickPoses(Node):
def get_grasp_pick_poses(
self, request: GetPickPlacePoses.Request, response: GetPickPlacePoses.Response
) -> GetPickPlacePoses.Response:
- if request.action_name == "pick":
- pregrasp_pose = self.grasping.get("pregrasp_pose", None)
- grasp_pose = self.grasping.get("grasp_pose", None)
- postgrasp_pose = self.grasping.get("postgrasp_pose", None)
+ if request.action_name == "to_graver.pick":
+ to_graver = self.grasping.get("to_graver", None)
+ pregrasp_pose = to_graver.get("pregrasp_pose", None)
+ grasp_pose = to_graver.get("grasp_pose", None)
+ postgrasp_pose = to_graver.get("postgrasp_pose", None)
if None in [pregrasp_pose, grasp_pose, postgrasp_pose]:
response.ok = False
@@ -60,11 +61,49 @@ class GetGraspPickPoses(Node):
self.create_pose(grasp_pose),
self.create_pose(postgrasp_pose),
]
+ elif request.action_name == "to_graver.place":
+ to_graver = self.grasping.get("to_graver", None)
+ preplace_pose = to_graver.get("preplace_pose", None)
+ place_pose = to_graver.get("place_pose", None)
+ postplace_pose = to_graver.get("postplace_pose", None)
- elif request.action_name == "place":
- preplace_pose = self.grasping.get("preplace_pose", None)
- place_pose = self.grasping.get("place_pose", None)
- postplace_pose = self.grasping.get("postplace_pose", None)
+ if None in [preplace_pose, place_pose, postplace_pose]:
+ response.ok = False
+ self.get_logger().error(
+ "Missing one or more of the preplace, place, or postplace poses."
+ )
+ return response
+
+ # Create Pose messages for placing
+ response.place = [
+ self.create_pose(preplace_pose),
+ self.create_pose(place_pose),
+ self.create_pose(postplace_pose),
+ ]
+ elif request.action_name == "from_graver.pick":
+ from_graver = self.grasping.get("from_graver", None)
+ pregrasp_pose = from_graver.get("pregrasp_pose", None)
+ grasp_pose = from_graver.get("grasp_pose", None)
+ postgrasp_pose = from_graver.get("postgrasp_pose", None)
+
+ if None in [pregrasp_pose, grasp_pose, postgrasp_pose]:
+ response.ok = False
+ self.get_logger().error(
+ "Missing one or more of the pregrasp, grasp, or postgrasp poses."
+ )
+ return response
+
+ # Create Pose messages
+ response.grasp = [
+ self.create_pose(pregrasp_pose),
+ self.create_pose(grasp_pose),
+ self.create_pose(postgrasp_pose),
+ ]
+ elif request.action_name == "from_graver.place":
+ from_graver = self.grasping.get("from_graver", None)
+ preplace_pose = from_graver.get("preplace_pose", None)
+ place_pose = from_graver.get("place_pose", None)
+ postplace_pose = from_graver.get("postplace_pose", None)
if None in [preplace_pose, place_pose, postplace_pose]:
response.ok = False
diff --git a/rbs_mill_assist/urdf/rbs_arm_macro.xacro b/rbs_mill_assist/urdf/rbs_arm_macro.xacro
index e6c17c1..87661c3 100644
--- a/rbs_mill_assist/urdf/rbs_arm_macro.xacro
+++ b/rbs_mill_assist/urdf/rbs_arm_macro.xacro
@@ -505,7 +505,7 @@
-
+
@@ -520,14 +520,95 @@
xyz="0 0 0" />
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
true
-
-
+
+
+
+
@@ -556,17 +637,17 @@
-
+
- Link8_collision
+ left_contact_panel_collision
-
+
- Link7_collision
+ right_contact_panel_collision