P3 final

spot/src/praktikum3/map/map.yaml

@@ -0,0 +1,7 @@
+image: map.pgm
+mode: trinary
+resolution: 0.05
+origin: [-1.22, -4.94, 0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.25

spot/src/praktikum3/package.xml

@@ -12,6 +12,10 @@
   <test_depend>ament_pep257</test_depend>
   <test_depend>python3-pytest</test_depend>
 
+  <exec_depend>nav2_msgs</exec_depend>
+  <exec_depend>webots_spot_msgs</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+
   <export>
     <build_type>ament_python</build_type>
   </export>

spot/src/praktikum3/praktikum3/p3_nav.py

@@ -0,0 +1,71 @@
+import rclpy
+from rclpy.node import Node
+from rclpy.action import ActionClient
+from nav2_msgs.action import NavigateToPose
+from praktikum3.p3_pushup import Pushups
+
+class MazeNavigator(Node):
+    def __init__(self):
+        super().__init__('maze_nav_node')
+        self._action_client = ActionClient(self, NavigateToPose, 'navigate_to_pose')
+
+    def go_to_goal(self, x, y, w):
+        self.get_logger().info(f'Navigiere zu {x}, {y}...')
+        
+        # Definiere Ziel
+        goal_msg = NavigateToPose.Goal()
+        goal_msg.pose.header.frame_id = 'map'
+        goal_msg.pose.header.stamp = self.get_clock().now().to_msg()
+        goal_msg.pose.pose.position.x = x
+        goal_msg.pose.pose.position.y = y
+        goal_msg.pose.pose.orientation.w = w
+        
+        # Handshake mit dem Nav2 Server
+        while not self._action_client.wait_for_server(timeout_sec=2.0):
+            self.get_logger().info('Nav2 Server antwortet nicht... läuft nav_launch.py?')
+        
+        # Übermittle Ziel zur Ermittlung der Gültigkeit
+        goal_to_be_checked = self._action_client.send_goal_async(goal_msg)
+        rclpy.spin_until_future_complete(self, goal_to_be_checked)
+        
+        # Prüfe Gültigkeit des Ziels
+        goal_result_of_check = goal_to_be_checked.result()
+        
+        if not goal_result_of_check.accepted:
+            self.get_logger().error('Ziel abgelehnt!')
+            return False
+        
+        # Navigiere zum Ziel
+        goal_result_of_reaching = goal_result_of_check.get_result_async()
+        rclpy.spin_until_future_complete(self, goal_result_of_reaching) # WARTEN bis angekommen
+        
+        if goal_result_of_reaching.result().status == 4: # Erfolgreich
+            self.get_logger().info('Am Ziel angekommen!')
+            return True
+        else:
+            return False
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    # 1. NAVIGATION
+    navigator = MazeNavigator()
+    success = navigator.go_to_goal(7.5, 1.0, 1.0) 
+    navigator.destroy_node() 
+    
+    # 2. PUSHUPS 
+    if success:
+        print("Starte Pushup-Sequenz...")
+        pushup_node = Pushups() # Nutze Klasse auf Aufgabe 3.1
+        
+        try:
+            rclpy.spin(pushup_node)
+        except KeyboardInterrupt:
+            print("Strg + C durch Benutzer")
+        finally:
+            pushup_node.destroy_node()
+
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

spot/src/praktikum3/praktikum3/p3_pushup.py

@@ -1,34 +1,42 @@
 import rclpy
 from rclpy.node import Node
-from rclpy.qos import QoSProfile
 from webots_spot_msgs.srv import SpotMotion
 
-class SubscriberClientNodeAsync(Node):
+class Pushups(Node):
     def __init__(self):
-        super().__init__('sub_client_node')
+        super().__init__('pushup_node') # Name konsistent gemacht
         self.sit_cli = self.create_client(SpotMotion, '/Spot/lie_down')
         self.stand_cli = self.create_client(SpotMotion, '/Spot/stand_up')
+        
+        # Handshake mit dem Server
+        while not self.sit_cli.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('/Spot/lie_down Server antwortet nicht...')
+        
         self.req = SpotMotion.Request()
         self.is_sitting = False
 
-        # Erstelle einen Timer, der alle 2 Sekunden die pushup_callback aufruft
+        # Timer startet direkt
         self.timer = self.create_timer(2.0, self.pushup_callback)
 
     def pushup_callback(self):
         if self.is_sitting:
             self.future = self.stand_cli.call_async(self.req)
-            self.get_logger().info("Standing up...")
+            self.get_logger().info("Up")
             self.is_sitting = False
         else:
             self.future = self.sit_cli.call_async(self.req)
-            self.get_logger().info("Lying down...")
+            self.get_logger().info("Down")
             self.is_sitting = True
 
 def main(args=None):
     rclpy.init(args=args)
-    pushupcli = SubscriberClientNodeAsync()
-    rclpy.spin(pushupcli)
-    pushupcli.destroy_node()
+    pushup_node = Pushups()
+    try:
+        rclpy.spin(pushup_node)
+    except KeyboardInterrupt:
+        print("Strg + C durch Benutzer")
+    finally:
+        pushup_node.destroy_node()
         rclpy.shutdown()
 
 if __name__ == '__main__':

spot/src/praktikum3/setup.py

@@ -1,3 +1,5 @@
+import os
+from glob import glob
 from setuptools import find_packages, setup
 
 package_name = 'praktikum3'
@@ -10,6 +12,11 @@ setup(
         ('share/ament_index/resource_index/packages',
             ['resource/' + package_name]),
         ('share/' + package_name, ['package.xml']),
+        # Launch Files kopieren
+        (os.path.join("share", package_name, "launch"), glob("launch/*_launch.py")),
+        # WICHTIG: Map Files kopieren
+        # Damit landen map.yaml und map.pgm/png in share/praktikum3/map/
+        # (os.path.join("share", package_name, "map"), glob("map/*")),
     ],
     install_requires=['setuptools'],
     zip_safe=True,
@@ -24,7 +31,8 @@ setup(
     },
     entry_points={
         'console_scripts': [
-            'p3_pushup = praktikum3.p3_pushup:main'
+            'p3_pushup = praktikum3.p3_pushup:main',
+            'p3_nav = praktikum3.p3_nav:main'
         ],
     },
 )