asssembly config simple publisher

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()

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()

rbs_bt_executor/bt_trees/test_roboclone.xml

@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<root main_tree_to_execute="MainTree">
+    <!-- ////////// -->
+    <BehaviorTree ID="MainTree">
+        <Sequence>
+            <Action ID="EnvStarter" env_class="gz_enviroment::GzEnviroment" env_name="gz_enviroment"
+                server_name="/env_manager/start_env" server_timeout="1000" workspace="{workspace}" />
+            <SubTreePlus ID="WorkspaceInspection" __autoremap="1" goal_pose="{workspace}"
+                robot_name="rbs_arm" />
+        </Sequence>
+    </BehaviorTree>
+    <!-- ////////// -->
+    <BehaviorTree ID="WorkspaceInspection">
+        <Sequence>
+            <Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
+                pose_vec_out="{goal_pose}"
+        robot_name="{robot_name}" server_name="/arm1/move_topose" server_timeout="5000" />
+            <Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
+                pose_vec_out="{goal_pose}"
+        robot_name="{robot_name}" server_name="/arm1/move_cartesian" server_timeout="5000" />
+            <Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
+                pose_vec_out="{goal_pose}"
+        robot_name="{robot_name}" server_name="/arm1/move_cartesian" server_timeout="5000" />
+            <Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
+                pose_vec_out="{goal_pose}"
+        robot_name="{robot_name}" server_name="/arm1/move_cartesian" server_timeout="5000" />
+            <Action ID="MoveToPoseArray" cancel_timeout="5000" pose_vec_in="{goal_pose}"
+                pose_vec_out="{goal_pose}"
+        robot_name="{robot_name}" server_name="/arm/move_cartesian" server_timeout="5000" />
+
+        </Sequence>
+    </BehaviorTree>
+    <!-- ////////// -->
+  <include path="./nodes_interfaces/general.xml"/>
+    <!-- ////////// -->
+</root>

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()