From 7c26b764d0185b287a4838e2888bdc444cb6cd7c Mon Sep 17 00:00:00 2001
From: Niko <nikolaus.feith@unileoben.ac.at>
Date: Tue, 12 Sep 2023 15:02:26 +0200
Subject: [PATCH] All components for Inverse Control implemented and tested +
 publishing the current endeffector state

---
 franka_iml_experiment/dmp_node.py    |  1 +
 franka_iml_experiment/move_square.py | 49 ++++++++++++++++++++++++++++
 setup.py                             |  1 +
 3 files changed, 51 insertions(+)
 create mode 100644 franka_iml_experiment/move_square.py

diff --git a/franka_iml_experiment/dmp_node.py b/franka_iml_experiment/dmp_node.py
index 55cc825..b8b3c44 100644
--- a/franka_iml_experiment/dmp_node.py
+++ b/franka_iml_experiment/dmp_node.py
@@ -9,6 +9,7 @@ from active_bo_msgs.msg import DMP
 import pydmps
 import pydmps.dmp_discrete
 
+
 import numpy as np
 
 
diff --git a/franka_iml_experiment/move_square.py b/franka_iml_experiment/move_square.py
new file mode 100644
index 0000000..2a9c5a5
--- /dev/null
+++ b/franka_iml_experiment/move_square.py
@@ -0,0 +1,49 @@
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import PoseArray, Pose, Point, Quaternion
+from std_msgs.msg import Empty
+
+
+class MoveFrankaSquareNode(Node):
+
+    def __init__(self):
+        super().__init__('move_franka_square')
+        self.publisher_ = self.create_publisher(PoseArray, 'moveit_interface/task_space_trajectory', 10)
+        self.subscriber_ = self.create_subscription(Empty, 'moveit_interface/trigger', self.trigger_callback, 10)
+
+    def trigger_callback(self, msg):
+        # Create a PoseArray message
+        pose_array = PoseArray()
+
+        # Define the four poses in the square
+        z_offset = 0.10  # 10 cm above the base joint
+        side_length = 0.5
+
+        poses = [
+            Pose(position=Point(x=0.4, y=0.3, z=0.15), orientation=Quaternion(x=1., y=0., z=0., w=0.)),
+            Pose(position=Point(x=0.4, y=-0.3, z=0.15), orientation=Quaternion(x=1., y=0., z=0., w=0.)),
+            # Pose(position=Point(x=-0.1, y=0.1, z=z_offset), de=Quaternion(x=0., y=1., z=0., w=0.)),
+            # Pose(position=Point(x=-0.1, y=-0.1, z=z_offset), orientation=Quaternion(x=0., y=1., z=0., w=0.))
+        ]
+
+        pose_array.poses = poses
+
+        # Publish the PoseArray
+        self.publisher_.publish(pose_array)
+        self.get_logger().info('Published PoseArray to moveit_interface/task_space_trajectory')
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    move_franka_square_node = MoveFrankaSquareNode()
+
+    rclpy.spin(move_franka_square_node)
+
+    move_franka_square_node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
diff --git a/setup.py b/setup.py
index db1b3de..0186ba8 100644
--- a/setup.py
+++ b/setup.py
@@ -25,6 +25,7 @@ setup(
         'console_scripts': [
             'dmp_node = franka_iml_experiment.dmp_node:main',
             'dmp_test = franka_iml_experiment.dmp_test:main',
+            'move_square = franka_iml_experiment.move_square:main',
         ],
     },
 )