diff --git a/rbs_mill_assist/bt/CMakeLists.txt b/rbs_mill_assist/bt/CMakeLists.txt index 1d6a06b..e038ae5 100644 --- a/rbs_mill_assist/bt/CMakeLists.txt +++ b/rbs_mill_assist/bt/CMakeLists.txt @@ -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) diff --git a/rbs_mill_assist/include/rbs_mill_assist/places_config.hpp b/rbs_mill_assist/include/rbs_mill_assist/places_config.hpp index ce0de96..db7ce36 100644 --- a/rbs_mill_assist/include/rbs_mill_assist/places_config.hpp +++ b/rbs_mill_assist/include/rbs_mill_assist/places_config.hpp @@ -2,7 +2,10 @@ #include #include #include +#include #include +#include +#include #include #include #include @@ -25,12 +28,16 @@ public: inline std::vector getModelNames() { return _model_names; } + inline std::vector getModelParents() { + return _model_parents; + } private: std::vector _model_names; std::vector _model_mesh_filenames; std::vector _model_poses; std::vector _model_mesh_filepaths; + std::vector _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 model_parent = "world"; + if (model["relative_to"].IsDefined()) { + model_parent = model["relative_to"].as(); + } std::string model_mesh_filename = model["mesh_filename"].as(); _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) / diff --git a/rbs_mill_assist/launch/moveit.launch.py b/rbs_mill_assist/launch/moveit.launch.py index 8111d14..803e22b 100644 --- a/rbs_mill_assist/launch/moveit.launch.py +++ b/rbs_mill_assist/launch/moveit.launch.py @@ -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 diff --git a/rbs_mill_assist/launch/simulation.launch.py b/rbs_mill_assist/launch/simulation.launch.py index 6678104..e8f152f 100644 --- a/rbs_mill_assist/launch/simulation.launch.py +++ b/rbs_mill_assist/launch/simulation.launch.py @@ -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 = [ diff --git a/rbs_mill_assist/scripts/get_key_pose_frame.py b/rbs_mill_assist/scripts/get_key_pose_frame.py index 790fee0..ce81dde 100755 --- a/rbs_mill_assist/scripts/get_key_pose_frame.py +++ b/rbs_mill_assist/scripts/get_key_pose_frame.py @@ -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 diff --git a/rbs_mill_assist/scripts/grasping_service.py b/rbs_mill_assist/scripts/grasping_service.py index 86b75a9..aff6038 100755 --- a/rbs_mill_assist/scripts/grasping_service.py +++ b/rbs_mill_assist/scripts/grasping_service.py @@ -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()