From a3a1579d282ca40465d9adf668027b94605a1c01 Mon Sep 17 00:00:00 2001
From: Niko <nikolaus.feith@unileoben.ac.at>
Date: Fri, 26 Jan 2024 17:26:11 +0100
Subject: [PATCH] move_square tested

---
 franka_iml_experiment/CheckExperimentPose.py  | 64 +++++++++++++++++++
 franka_iml_experiment/iml_experiment_nodmp.py | 12 ++--
 setup.py                                      |  1 +
 3 files changed, 73 insertions(+), 4 deletions(-)
 create mode 100644 franka_iml_experiment/CheckExperimentPose.py

diff --git a/franka_iml_experiment/CheckExperimentPose.py b/franka_iml_experiment/CheckExperimentPose.py
new file mode 100644
index 0000000..2304b4d
--- /dev/null
+++ b/franka_iml_experiment/CheckExperimentPose.py
@@ -0,0 +1,64 @@
+
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import PoseArray, Pose, Point, Quaternion
+from std_msgs.msg import Int16, Empty
+
+
+class CheckExperimentPose(Node):
+
+    def __init__(self):
+        super().__init__('check_experiment_pose')
+        self.publisher_ = self.create_publisher(PoseArray, 'moveit_interface/task_space_trajectory', 10)
+        self.subscriber_ = self.create_subscription(Int16, 'moveit_interface/trigger', self.trigger_callback, 10)
+        self.pub_rviz_model = self.create_publisher(Empty, 'moveit_interface/box', 10)
+        msg = Empty()
+        self.pub_rviz_model.publish(msg)
+
+    def trigger_callback(self, msg):
+        data = msg.data
+        if data == 1:
+            # start
+            pose = Pose(position=Point(x=0.5, y=-0.3, z=0.15), orientation=Quaternion(x=1., y=0., z=0., w=0.)),
+        elif data == 2:
+            # end
+            pose = Pose(position=Point(x=0.5, y=0.3, z=0.15), orientation=Quaternion(x=1., y=0., z=0., w=0.)),
+        elif data == 3:
+            # square 1
+            pose = Pose(position=Point(x=0.4, y=0.1, z=0.15), orientation=Quaternion(x=1., y=0., z=0., w=0.)),
+        elif data == 4:
+            # square 2
+            pose = Pose(position=Point(x=0.4, y=-0.1, z=0.15), orientation=Quaternion(x=1., y=0., z=0., w=0.)),
+        elif data == 5:
+            # square 3
+            pose = Pose(position=Point(x=0.6, y=-0.1, z=0.15), orientation=Quaternion(x=1., y=0., z=0., w=0.)),
+        elif data == 6:
+            # square 4
+            pose = Pose(position=Point(x=0.6, y=0.1, z=0.15), orientation=Quaternion(x=1., y=0., z=0., w=0.)),
+        else:
+            self.get_logger().warn(f'Pose: {data} is not defined!')
+            pose = Pose(position=Point(x=0.5, y=-0.3, z=0.15), orientation=Quaternion(x=1., y=0., z=0., w=0.)),
+
+        # Create a PoseArray message
+        pose_array = PoseArray()
+
+        pose_array.poses = pose
+
+        # 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)
+
+    check_experiment_pose_node = CheckExperimentPose()
+
+    rclpy.spin(check_experiment_pose_node)
+
+    check_experiment_pose_node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()
diff --git a/franka_iml_experiment/iml_experiment_nodmp.py b/franka_iml_experiment/iml_experiment_nodmp.py
index 8d7eced..86907ad 100644
--- a/franka_iml_experiment/iml_experiment_nodmp.py
+++ b/franka_iml_experiment/iml_experiment_nodmp.py
@@ -54,6 +54,10 @@ class IML_Experiment(Node):
                                                  1,
                                                  callback_group=topic_callback_group)
 
+        self.pub_rviz_model = self.create_publisher(Empty, 'moveit_interface/box', 10)
+        msg = Empty()
+        self.pub_rviz_model.publish(msg)
+
         # States
         self.eval_pending = False
 
@@ -65,13 +69,13 @@ class IML_Experiment(Node):
         self.goal_rew = 10.
 
         # Scene parameters
-        self.square_center = np.array([0.5, 0.0])
+        self.square_center = np.array([0.55, 0.0])
         self.square_half_side = 0.1  # half of the side length
-        self.circle_radius = 0.05
+        self.circle_radius = 0.02
 
         # BO parameters
-        self.start = {0: [0.5, -0.3]}
-        self.end = [0.5, 0.3]
+        self.start = {0: [0.55, -0.3]}
+        self.end = [0.55, 0.3]
         self.nr_dims = None
         self.nr_weights = None
         self.nr_steps = None
diff --git a/setup.py b/setup.py
index 5b89959..bd6babc 100644
--- a/setup.py
+++ b/setup.py
@@ -29,6 +29,7 @@ setup(
             'dmp_test = franka_iml_experiment.dmp_test:main',
             'move_square = franka_iml_experiment.move_square:main',
             'iml_experiment = franka_iml_experiment.iml_experiment_nodmp:main',
+            'check_experiment_pose = franka_iml_experiment.CheckExperimentPose:main'
         ],
     },
 )