From 662e20faa26f093411bc3339d7a9355bf8ef3a6b Mon Sep 17 00:00:00 2001
From: Alexander Lehr <contact@alexander-lehr.com>
Date: Mon, 27 Nov 2023 17:25:14 +0100
Subject: [PATCH] Aufgabe 5 push

---
 Aufgabe_5.py | 99 ++++++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 99 insertions(+)
 create mode 100644 Aufgabe_5.py

diff --git a/Aufgabe_5.py b/Aufgabe_5.py
new file mode 100644
index 0000000..e39389f
--- /dev/null
+++ b/Aufgabe_5.py
@@ -0,0 +1,99 @@
+import roslibpy.actionlib
+
+client = roslibpy.Ros(host='10.10.10.10', port=9090)
+client.run()
+
+print(f"Is ROS connected? {client.is_connected}")
+
+
+def request(service_name, type, value=None):
+    service = roslibpy.Service(client, service_name, type)
+    request = roslibpy.ServiceRequest(value)
+    return service.call(request)
+
+
+request('niryo_robot_tools_commander/update_tool', 'std_srvs/Trigger')
+request('niryo_robot_tools_commander/enable_tcp', "std_srvs/Bool", {'value': True})
+
+
+action = roslibpy.actionlib.ActionClient(client, '/niryo_robot_arm_commander/robot_action', '/niryo_robot_arm_commander/RobotMoveAction')
+goal = roslibpy.actionlib.Goal(action, roslibpy.Message({'cmd': {
+            'cmd_type': 1,
+            'position': {
+                'x': 0.2,
+                'y': 0.2,
+                'z': 0.02
+            },
+            'rpy': {
+                'roll': 0,
+                'pitch': 1.57,
+                'yaw': 0
+            }
+        }}))
+
+goal.send()
+goal.wait(10)
+
+
+action = roslibpy.actionlib.ActionClient(client, "/niryo_robot_tools_commander/action_server", '/niryo_robot_tools_commander/ToolAction')
+goal = roslibpy.actionlib.Goal(action, roslibpy.Message({
+    'cmd': {
+        'cmd_type': 2,
+        'tool_id': 11,
+        'max_torque_percentage': 100,
+        'hold_torque_percentage': 30
+    }
+}))
+
+goal.send()
+goal.wait(10)
+
+action = roslibpy.actionlib.ActionClient(client, '/niryo_robot_arm_commander/robot_action', '/niryo_robot_arm_commander/RobotMoveAction')
+goal = roslibpy.actionlib.Goal(action, roslibpy.Message({'cmd': {
+            'cmd_type': 1,
+            'position': {
+                'x': 0.2,
+                'y': 0.2,
+                'z': 0.2
+            },
+            'rpy': {
+                'roll': 0,
+                'pitch': 1.57,
+                'yaw': 0
+            }
+        }}))
+
+goal.send()
+goal.wait(10)
+
+action = roslibpy.actionlib.ActionClient(client, '/niryo_robot_arm_commander/robot_action', '/niryo_robot_arm_commander/RobotMoveAction')
+goal = roslibpy.actionlib.Goal(action, roslibpy.Message({'cmd': {
+            'cmd_type': 1,
+            'position': {
+                'x': -0.2,
+                'y': -0.2,
+                'z': 0.02
+            },
+            'rpy': {
+                'roll': 0,
+                'pitch': 1.57,
+                'yaw': 0
+            }
+        }}))
+
+goal.send()
+goal.wait(10)
+
+action = roslibpy.actionlib.ActionClient(client, "/niryo_robot_tools_commander/action_server", '/niryo_robot_tools_commander/ToolAction')
+goal = roslibpy.actionlib.Goal(action, roslibpy.Message({
+    'cmd': {
+        'cmd_type': 1,
+        'tool_id': 11,
+        'max_torque_percentage': 100,
+        'hold_torque_percentage': 30
+    }
+}))
+
+goal.send()
+goal.wait(10)
+