From ae48c5ee6a804556500bf67aaaf0cc6678fffeb3 Mon Sep 17 00:00:00 2001 From: Bill Finger Date: Mon, 22 Apr 2024 15:01:40 +0300 Subject: [PATCH] asssembly config simple publisher --- .../scripts/publish_asm_config.py | 114 ++++++++++++++++++ env_manager/gz_enviroment/scripts/test_tf.py | 75 ++++++++++++ rbs_bt_executor/bt_trees/test_roboclone.xml | 36 ++++++ rbs_skill_servers/scripts/move_to_pose.py | 108 +++++++++++++++++ 4 files changed, 333 insertions(+) create mode 100755 env_manager/gz_enviroment/scripts/publish_asm_config.py create mode 100755 env_manager/gz_enviroment/scripts/test_tf.py create mode 100644 rbs_bt_executor/bt_trees/test_roboclone.xml create mode 100755 rbs_skill_servers/scripts/move_to_pose.py diff --git a/env_manager/gz_enviroment/scripts/publish_asm_config.py b/env_manager/gz_enviroment/scripts/publish_asm_config.py new file mode 100755 index 0000000..dbc1931 --- /dev/null +++ b/env_manager/gz_enviroment/scripts/publish_asm_config.py @@ -0,0 +1,114 @@ +#! /usr/bin/env python3 + +import yaml +import rclpy +from rclpy.node import Node +from rbs_utils_interfaces.msg import AssemblyConfig, NamedPose, RelativeNamedPose +from geometry_msgs.msg import Point +import os + + +class ConfigPublisherNode(Node): + + def __init__(self): + super().__init__('config_publisher_node') + self.publisher_ = self.create_publisher(AssemblyConfig, '/assembly_config', 10) + timer_period = 1 # seconds + self.timer = self.create_timer(timer_period, self.timer_callback) + if 'RBS_ASSEMBLY_DIR' in os.environ: + assembly_dir = os.environ['RBS_ASSEMBLY_DIR'] + else: + assembly_dir = '~/assembly/robossembler-stuff' + self.assembly_dir = os.path.expanduser(assembly_dir) + datafolder = "asp-example" + # Read data from YAML file + with open(f"{self.assembly_dir}/{datafolder}/rbs_db.yaml") as f: + self.data = yaml.safe_load(f) + + def timer_callback(self): + + # Create AssemblyConfig message + msg = AssemblyConfig() + # Fill in the data from the YAML file + msg.assets_db = self.data['assets_db'] + + # Populate workspace + for wp in self.data['workspace']: + point = Point() + point.x = wp["x"] + point.y = wp["y"] + point.z = wp["z"] + msg.workspace.append(point) + + # Populate absolute_part + for part in self.data['absolute_part']: + named_pose = NamedPose() + named_pose.name = part['name'] + pose = part['pose'] + named_pose.pose.position.x = pose['position']['x'] + named_pose.pose.position.y = pose['position']['y'] + named_pose.pose.position.z = pose['position']['z'] + named_pose.pose.orientation.x = pose['orientation']['x'] + named_pose.pose.orientation.y = pose['orientation']['y'] + named_pose.pose.orientation.z = pose['orientation']['z'] + named_pose.pose.orientation.w = pose['orientation']['w'] + msg.absolute_part.append(named_pose) + + # Populate relative_part + for part in self.data['relative_part']: + relative_named_pose = RelativeNamedPose() + relative_named_pose.name = part['name'] + relative_named_pose.relative_at = part['relative_at'] + pose = part['pose'] + relative_named_pose.pose.position.x = pose['position']['x'] + relative_named_pose.pose.position.y = pose['position']['y'] + relative_named_pose.pose.position.z = pose['position']['z'] + relative_named_pose.pose.orientation.x = pose['orientation']['x'] + relative_named_pose.pose.orientation.y = pose['orientation']['y'] + relative_named_pose.pose.orientation.z = pose['orientation']['z'] + relative_named_pose.pose.orientation.w = pose['orientation']['w'] + msg.relative_part.append(relative_named_pose) + + # Populate grasp_pose + for part in self.data['grasp_pose']: + relative_named_pose = RelativeNamedPose() + relative_named_pose.name = part['name'] + relative_named_pose.relative_at = part['relative_at'] + pose = part['pose'] + relative_named_pose.pose.position.x = pose['position']['x'] + relative_named_pose.pose.position.y = pose['position']['y'] + relative_named_pose.pose.position.z = pose['position']['z'] + relative_named_pose.pose.orientation.x = pose['orientation']['x'] + relative_named_pose.pose.orientation.y = pose['orientation']['y'] + relative_named_pose.pose.orientation.z = pose['orientation']['z'] + relative_named_pose.pose.orientation.w = pose['orientation']['w'] + msg.grasp_pose.append(relative_named_pose) + + # Populate extra_poses + for part in self.data['extra_poses']: + named_pose = NamedPose() + named_pose.name = part['name'] + pose = part['pose'] + named_pose.pose.position.x = pose['position']['x'] + named_pose.pose.position.y = pose['position']['y'] + named_pose.pose.position.z = pose['position']['z'] + named_pose.pose.orientation.x = pose['orientation']['x'] + named_pose.pose.orientation.y = pose['orientation']['y'] + named_pose.pose.orientation.z = pose['orientation']['z'] + named_pose.pose.orientation.w = pose['orientation']['w'] + msg.extra_poses.append(named_pose) + + # Publish the message + self.publisher_.publish(msg) + + +def main(args=None): + rclpy.init(args=args) + config_publisher_node = ConfigPublisherNode() + rclpy.spin(config_publisher_node) + config_publisher_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/env_manager/gz_enviroment/scripts/test_tf.py b/env_manager/gz_enviroment/scripts/test_tf.py new file mode 100755 index 0000000..d7f7ebb --- /dev/null +++ b/env_manager/gz_enviroment/scripts/test_tf.py @@ -0,0 +1,75 @@ +#! /usr/bin/env python3 + +import rclpy +from rclpy.node import Node +from tf2_msgs.msg import TFMessage +from geometry_msgs.msg import TransformStamped +from rbs_utils_interfaces.msg import AssemblyConfig +from tf2_ros.transform_broadcaster import TransformBroadcaster + + +class TF2PublisherNode(Node): + + def __init__(self): + super().__init__('tf2_publisher_node') + self.tfb_ = TransformBroadcaster(self) + self.subscription = self.create_subscription( + AssemblyConfig, '/assembly_config', self.config_callback, 10) + + def config_callback(self, msg): + # Populate transforms for absolute_part + for part in msg.absolute_part: + ts = TransformStamped() + ts.header.stamp = self.get_clock().now().to_msg() + ts.header.frame_id = "world" + ts.child_frame_id = part.name + ts.transform.translation.x = part.pose.position.x + ts.transform.translation.y = part.pose.position.y + ts.transform.translation.z = part.pose.position.z + ts.transform.rotation.x = part.pose.orientation.x + ts.transform.rotation.y = part.pose.orientation.y + ts.transform.rotation.z = part.pose.orientation.z + ts.transform.rotation.w = part.pose.orientation.w + self.tfb_.sendTransform(ts) + + # Populate transforms for relative_part + for part in msg.relative_part: + ts = TransformStamped() + ts.header.stamp = self.get_clock().now().to_msg() + ts.header.frame_id = part.relative_at + ts.child_frame_id = part.name + ts.transform.translation.x = part.pose.position.x + ts.transform.translation.y = part.pose.position.y + ts.transform.translation.z = part.pose.position.z + ts.transform.rotation.x = part.pose.orientation.x + ts.transform.rotation.y = part.pose.orientation.y + ts.transform.rotation.z = part.pose.orientation.z + ts.transform.rotation.w = part.pose.orientation.w + self.tfb_.sendTransform(ts) + + # Populate transforms for grasp_pose + for part in msg.grasp_pose: + ts = TransformStamped() + ts.header.stamp = self.get_clock().now().to_msg() + ts.header.frame_id = part.relative_at + ts.child_frame_id = part.name + ts.transform.translation.x = part.pose.position.x + ts.transform.translation.y = part.pose.position.y + ts.transform.translation.z = part.pose.position.z + ts.transform.rotation.x = part.pose.orientation.x + ts.transform.rotation.y = part.pose.orientation.y + ts.transform.rotation.z = part.pose.orientation.z + ts.transform.rotation.w = part.pose.orientation.w + self.tfb_.sendTransform(ts) + + +def main(args=None): + rclpy.init(args=args) + tf2_publisher_node = TF2PublisherNode() + rclpy.spin(tf2_publisher_node) + tf2_publisher_node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() diff --git a/rbs_bt_executor/bt_trees/test_roboclone.xml b/rbs_bt_executor/bt_trees/test_roboclone.xml new file mode 100644 index 0000000..6fb5741 --- /dev/null +++ b/rbs_bt_executor/bt_trees/test_roboclone.xml @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rbs_skill_servers/scripts/move_to_pose.py b/rbs_skill_servers/scripts/move_to_pose.py new file mode 100755 index 0000000..e29208e --- /dev/null +++ b/rbs_skill_servers/scripts/move_to_pose.py @@ -0,0 +1,108 @@ +#!/usr/bin/python +import rclpy +from rclpy.action import ActionServer +from rclpy.node import Node +import numpy as np +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.executors import MultiThreadedExecutor + +from geometry_msgs.msg import Pose, PoseStamped +from rbs_skill_interfaces.action import MoveitSendPose + +class PoseSubscriber(Node): + def __init__(self, parent=None): + super().__init__('pose_subscriber') + self.parent = parent + self._sub = self.create_subscription(PoseStamped, + "/cartesian_motion_controller/current_pose", + self.parent.on_pose_callback, 1, + callback_group=self.parent._callback_group) + self.get_logger().info('PoseSubscriber node initialized') + +class CartesianMoveToPose(Node): + + def __init__(self): + super().__init__('cartesian_move_to_pose') + self._callback_group = ReentrantCallbackGroup() + self._action_server = ActionServer( + self, + MoveitSendPose, + 'cartesian_move_to_pose', + self.execute_callback, callback_group=self._callback_group) + self._pub = self.create_publisher(PoseStamped, + "/cartesian_motion_controller/target_frame", 1, + callback_group=self._callback_group) + self.current_pose = None + self.goal_tolerance = 0.05 + + def on_pose_callback(self, msg: PoseStamped): + if isinstance(msg, PoseStamped): + self.current_pose = msg + + def execute_callback(self, goal_handle): + self.get_logger().debug(f"Executing goal {goal_handle.request.target_pose}") + tp = PoseStamped() + tp.pose = goal_handle.request.target_pose + tp.header.stamp = self.get_clock().now().to_msg() + tp.header.frame_id = "base_link" + + while self.get_distance_to_target(tp.pose) >= self.goal_tolerance: + self._pub.publish(tp) + + goal_handle.succeed() + + result = MoveitSendPose.Result() + result.success = True + return result + + def get_distance_to_target(self, target_pose: Pose): + if self.current_pose is None or self.current_pose.pose is None: + self.get_logger().warn("Current pose is not available") + return None + + current_pose = self.current_pose.pose + + current_position = np.array([ + current_pose.position.x, + current_pose.position.y, + current_pose.position.z, + current_pose.orientation.x, + current_pose.orientation.y, + current_pose.orientation.z + ]) + + target_position = np.array([ + target_pose.position.x, + target_pose.position.y, + target_pose.position.z, + target_pose.orientation.x, + target_pose.orientation.y, + target_pose.orientation.z + ]) + + # Проверка на наличие значений в массивах координат + if np.any(np.isnan(current_position)) or np.any(np.isnan(target_position)): + self.get_logger().error("Invalid coordinates") + return None + + # Вычисляем расстояние между текущей и целевой позициями + distance = np.linalg.norm(current_position - target_position) + + return distance + +def main(args=None): + rclpy.init(args=args) + + cartesian_move_to_pose = CartesianMoveToPose() + pose_subscriber = PoseSubscriber(parent=cartesian_move_to_pose) + + executor = MultiThreadedExecutor() + executor.add_node(cartesian_move_to_pose) + executor.add_node(pose_subscriber) + + executor.spin() + + rclpy.shutdown() + +if __name__ == '__main__': + main()