robossembler/cnc-graver-assist
7
0
Fork 0
Code Issues 5 Pull requests 1 Projects Releases Packages Wiki Activity Actions

refactor: get poses from TF

Browse source
This commit is contained in:
Ilya Uraev 2025-04-07 12:43:31 +03:00
parent 4849bb8c09
commit 21baf934a9
6 changed files with 212 additions and 181 deletions
Download patch file Download diff file Expand all files Collapse all files

9
rbs_mill_assist/bt/CMakeLists.txt
View file

@ -5,6 +5,7 @@ find_package(rbs_skill_interfaces REQUIRED)
find_package(rbs_utils_interfaces REQUIRED)
find_package(geometry_msgs REQUIRED)
find_package(std_srvs REQUIRED)
find_package(rbs_mill_interfaces REQUIRED)
# Behaviortree interfaces
set(dependencies
@ -14,13 +15,17 @@ set(dependencies
behaviortree_ros2
rbs_utils_interfaces
std_srvs
rbs_mill_interfaces
)
add_library(vacuum_gripper_toggle SHARED plugins/vacuum_gripper_toggle.cpp)
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_grasp_poses SHARED plugins/get_grasp_pose.cpp)
list(APPEND plugin_libs get_grasp_poses)
add_library(get_place_poses SHARED plugins/get_place_pose.cpp)
list(APPEND plugin_libs get_place_poses)
add_library(get_named_pose SHARED plugins/get_named_pose.cpp)
list(APPEND plugin_libs get_named_pose)

12
rbs_mill_assist/include/rbs_mill_assist/places_config.hpp
View file

@ -2,7 +2,10 @@
#include <filesystem>
#include <rclcpp/logger.hpp>
#include <rclcpp/node.hpp>
#include <string>
#include <tf2/LinearMath/Quaternion.hpp>
#include <utility>
#include <vector>
#include <yaml-cpp/node/node.h>
#include <yaml-cpp/node/parse.h>
#include <yaml-cpp/yaml.h>
@ -25,12 +28,16 @@ public:
inline std::vector<std::string> getModelNames() {
return _model_names;
}
inline std::vector<std::string> getModelParents() {
return _model_parents;
}
private:
std::vector<std::string> _model_names;
std::vector<std::string> _model_mesh_filenames;
std::vector<geometry_msgs::msg::Pose> _model_poses;
std::vector<std::string> _model_mesh_filepaths;
std::vector<std::string> _model_parents;
bool isQuat(const YAML::Node &node) {
if (node["x"] && node["y"] && node["z"] && node["w"]) {
@ -81,11 +88,16 @@ private:
}
std::string model_name = model["name"].as<std::string>();
std::string model_parent = "world";
if (model["relative_to"].IsDefined()) {
model_parent = model["relative_to"].as<std::string>();
}
std::string model_mesh_filename =
model["mesh_filename"].as<std::string>();
_model_names.push_back(std::move(model_name));
_model_mesh_filenames.push_back(std::move(model_mesh_filename));
_model_parents.push_back(std::move(model_parent));
if (!model_dir.empty()) {
std::filesystem::path mesh_path = std::filesystem::path(model_dir) /

41
rbs_mill_assist/launch/moveit.launch.py
View file

@ -1,7 +1,8 @@
from launch.event_handlers import OnExecutionComplete, OnProcessExit, OnProcessStart
from launch_param_builder import get_package_share_directory
from launch_ros.actions import Node
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, OpaqueFunction
from launch.actions import DeclareLaunchArgument, OpaqueFunction, RegisterEventHandler, TimerAction
from launch.substitutions import (
LaunchConfiguration,
)
@ -117,7 +118,6 @@ def launch_setup(context, *args, **kwargs):
(
moveit_config.planning_pipelines["ompl"]
.setdefault("arm", {})
# .setdefault("planner_configs", {})
.update(
{"planner_configs":planners_ids}
)
@ -138,6 +138,33 @@ def launch_setup(context, *args, **kwargs):
parameters=[moveit_config.to_dict(), {"use_sim_time": True}, {"places_config": places_config_filepath}],
)
ref_frames_config_filepath = os.path.join(
get_package_share_directory("rbs_mill_assist"),
"world/config",
"ref_frames.yaml",
)
places_frame_publisher = Node(
package="rbs_mill_assist",
executable="places_frame_publisher",
parameters=[
{"use_sim_time": use_sim_time},
{"places_config_filepath": places_config_filepath},
{"ref_frames_config_filepath": ref_frames_config_filepath},
],
)
handler = RegisterEventHandler(
OnProcessStart(
target_action=places_frame_publisher,
on_start=[planning_scene_publisher],
)
)
rviz_node = Node(
package="rviz2",
executable="rviz2",
@ -155,10 +182,18 @@ def launch_setup(context, *args, **kwargs):
],
)
delay_planning_scene = TimerAction(
actions=[planning_scene_publisher],
period=5.0
)
nodes_to_start = [
move_group_node,
planning_scene_publisher,
rviz_node,
places_frame_publisher,
planning_scene_publisher,
# delay_planning_scene
# handler
]
return nodes_to_start

42
rbs_mill_assist/launch/simulation.launch.py
View file

@ -1,5 +1,6 @@
import os
from launch.conditions import IfCondition, UnlessCondition
import xacro
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
@ -175,7 +176,8 @@ def launch_setup(context, *args, **kwargs):
"-x", "0.0",
"-z", "0.0",
"-y", "0.0",
"-topic", "/robot_description",
# "-topic", "/robot_description",
"-string", robot_description_content
],
output="screen",
parameters=[{"use_sim_time": True}],
@ -191,42 +193,19 @@ def launch_setup(context, *args, **kwargs):
grasping_service = Node(package="rbs_mill_assist", executable="grasping_service.py")
get_named_pose_service = Node(
package="rbs_mill_assist", executable="get_key_pose_frame.py"
package="rbs_mill_assist",
executable="get_key_pose_frame.py",
parameters=[{"use_sim_time": use_sim_time}],
)
# get_workspace = Node(
# package="rbs_mill_assist",
# executable="get_workspace",
# parameters=[
# {
# "urdf_path": os.path.join(
# get_package_share_directory("rbs_mill_assist"),
# "urdf",
# "current.urdf",
# ),
# "ee_link": ee_link_name,
# "use_sim_time": True,
# "robot_position": [0.0, 0.0, 0.0],
# }
# ],
# )
#
# publish_ee_pose = Node(
# package="rbs_mill_assist",
# executable="publish_ee_pose_node",
# parameters=[
# {"use_sim_time": use_sim_time},
# {"base_link": base_link_name},
# {"ee_link": ee_link_name},
# ],
# )
places_config_filepath = os.path.join(
get_package_share_directory("rbs_mill_assist"), "world/config", "places.yaml"
)
ref_frames_config_filepath = os.path.join(
get_package_share_directory("rbs_mill_assist"), "world/config", "slots.yaml"
get_package_share_directory("rbs_mill_assist"),
"world/config",
"ref_frames.yaml",
)
places_frame_publisher = Node(
package="rbs_mill_assist",
@ -234,8 +213,9 @@ def launch_setup(context, *args, **kwargs):
parameters=[
{"use_sim_time": use_sim_time},
{"places_config_filepath": places_config_filepath},
{"ref_frames_config_filepath": ref_frames_config_filepath}
{"ref_frames_config_filepath": ref_frames_config_filepath},
],
condition=UnlessCondition(use_moveit)
)
nodes_to_start = [

73
rbs_mill_assist/scripts/get_key_pose_frame.py
View file

@ -1,57 +1,64 @@
#!/usr/bin/env python3
import os
from typing import Dict
# import os
# from typing import Dict
import rclpy
import yaml
from ament_index_python.packages import get_package_share_directory
# import tf2_py
# 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 rbs_utils_interfaces.srv import GetRelativeNamedPose
from rclpy.node import Node
from rclpy.service import Service
from tf2_py import TransformException
from tf2_ros import TransformStamped
from tf2_ros.buffer import Buffer
from tf2_ros.transform_listener import TransformListener
class GetNamedPoseService(Node):
def __init__(self) -> None:
super().__init__("get_named_pose_service")
super().__init__("get_tf_frame_pose_node")
self.srv: Service = self.create_service(
GetNamedPose, "get_named_pose", self.get_named_pose
GetRelativeNamedPose, "get_tf_frame_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
self.tf_buffer = Buffer()
self.tf_listner = TransformListener(self.tf_buffer, self)
def get_named_pose(
self, request: GetNamedPose.Request, response: GetNamedPose.Response
) -> GetNamedPose.Response:
self,
request: GetRelativeNamedPose.Request,
response: GetRelativeNamedPose.Response,
) -> GetRelativeNamedPose.Response:
t = TransformStamped()
key_pose = self.key_poses.get(request.pose_name, None)
if key_pose is None:
try:
t = self.tf_buffer.lookup_transform(
request.relative_to, request.pose_name, self.get_clock().now()
)
except TransformException as ex:
self.get_logger().info(
f"Could not transform from {request.pose_name} to {request.relative_to}: {ex}"
)
response.ok = False
return response
response.named_pose.name = request.pose_name
response.named_pose.pose = self.create_pose(key_pose)
response.ok = True
p = Pose()
p.position.x = t.transform.translation.x
p.position.y = t.transform.translation.y
p.position.z = t.transform.translation.z
p.orientation.x = t.transform.rotation.x
p.orientation.y = t.transform.rotation.y
p.orientation.z = t.transform.rotation.z
p.orientation.w = t.transform.rotation.w
response.named_pose.pose = p
response.named_pose.relative_to = request.relative_to
response.named_pose.name = request.pose_name
response.ok = True
return response

216
rbs_mill_assist/scripts/grasping_service.py
View file

@ -1,140 +1,132 @@
#!/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_skill_interfaces.srv import GetPickPlacePoses
import math
from tf2_ros import Buffer, TransformListener, TransformException, TransformStamped
from geometry_msgs.msg import Pose, Quaternion, Point
from rbs_mill_interfaces.srv import GetGraspingPose
from rclpy.node import Node
from rclpy.service import Service
from scipy.spatial.transform import Rotation as R
class GetGraspPickPoses(Node):
def __init__(self) -> None:
super().__init__("get_grasp_pick_poses")
self.srv: Service = self.create_service(
GetPickPlacePoses, "get_pick_place_poses", self.get_grasp_pick_poses
GetGraspingPose, "get_grasping_pose", self.get_grasp_pick_poses
)
yaml_file_path: str = os.path.join(
get_package_share_directory("rbs_mill_assist"), "config", "grasping.yaml"
)
with open(yaml_file_path, "r", encoding="utf-8") as file:
self.grasping: 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
self.tf_buffer = Buffer()
self.tf_listener = TransformListener(self.tf_buffer, self)
# Precompute the rotation correction quaternion
self.rotation_correction = self.euler_to_quat(0, math.pi, math.pi/2)
def get_grasp_pick_poses(
self, request: GetPickPlacePoses.Request, response: GetPickPlacePoses.Response
) -> GetPickPlacePoses.Response:
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)
self,
request: GetGraspingPose.Request,
response: GetGraspingPose.Response,
) -> GetGraspingPose.Response:
if request.action_type not in ("pick", "place"):
self.get_logger().error(f"Unsupported action type: {request.action_type}")
response.ok = False
return response
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
# Common logic for pick/place
tf = self.get_tf_pose(request.pose_name, request.relative_to)
#tf_base = self.get_tf_pose(request.pose_name, "base_link")
if tf is None:
response.ok = False
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 == "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)
# Apply transformations
main_pose, pre_pose, post_pose = self.process_tf_and_generate_poses(tf)
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
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),
]
if request.action_type == "pick":
response.grasp_poses.pose = main_pose
response.grasp_poses.pre_pose = pre_pose
response.grasp_poses.post_pose = post_pose
else:
response.place_poses.pose = main_pose
response.place_poses.pre_pose = pre_pose
response.place_poses.post_pose = post_pose
response.ok = True
self.get_logger().info(f"Handled action: {request.action_name}")
return response
def process_tf_and_generate_poses(self, tf: TransformStamped) -> tuple[Pose, Pose, Pose]:
"""Processes TF and generates main poses."""
# Apply rotation correction
tf.transform.rotation = self.multiply_quaternions(
tf.transform.rotation,
self.rotation_correction
)
pose = self.get_pose_from_tf(tf)
return pose, self.add_offset(pose, z=0.1), self.add_offset(pose, z=0.1)
def multiply_quaternions(self, q1: Quaternion, q2: Quaternion) -> Quaternion:
"""Multiplies two quaternions using scipy."""
r1 = R.from_quat([q1.x, q1.y, q1.z, q1.w])
r2 = R.from_quat([q2.x, q2.y, q2.z, q2.w])
result = (r1 * r2).as_quat() # type: ignore
return Quaternion(x=result[0], y=result[1], z=result[2], w=result[3])
def add_offset(self, pose: Pose, x: float = 0.0, y: float = 0.0, z: float = 0.0) -> Pose:
"""Creates a new pose with offset."""
return Pose(
position=Point(
x=pose.position.x + x,
y=pose.position.y + y,
z=pose.position.z + z
),
orientation=pose.orientation
)
def euler_to_quat(self, roll: float, pitch: float, yaw: float) -> Quaternion:
"""Converts Euler angles to quaternion."""
q = R.from_euler("xyz", [roll, pitch, yaw], degrees=False).as_quat()
return Quaternion(x=q[0], y=q[1], z=q[2], w=q[3])
def get_pose_from_tf(self, tf: TransformStamped) -> Pose:
"""Creates Pose from TransformStamped."""
return Pose(
position=Point(
x=tf.transform.translation.x,
y=tf.transform.translation.y,
z=tf.transform.translation.z
),
orientation=Quaternion(
x=tf.transform.rotation.x,
y=tf.transform.rotation.y,
z=tf.transform.rotation.z,
w=tf.transform.rotation.w
)
)
def get_tf_pose(self, source_frame: str, target_frame: str) -> TransformStamped | None:
"""Gets coordinate transform between frames."""
try:
return self.tf_buffer.lookup_transform(
target_frame,
source_frame,
rclpy.time.Time()
)
except TransformException as ex:
self.get_logger().error(
f"Transform error: {target_frame} -> {source_frame}: {ex}"
)
return None
def main():
rclpy.init()
executor = rclpy.executors.SingleThreadedExecutor()
# executor = rclpy.executors.MultiThreadedExecutor() # can't be used
i_node = GetGraspPickPoses()
executor.add_node(i_node)
node = GetGraspPickPoses()
try:
executor.spin()
except (KeyboardInterrupt, rclpy.executors.ExternalShutdownException):
i_node.destroy_node()
rclpy.spin(node)
except KeyboardInterrupt:
pass
finally:
node.destroy_node()
rclpy.shutdown()
if __name__ == "__main__":
main()