diff --git a/spot/src/praktikum3/map/map.pgm b/spot/src/praktikum3/map/map.pgm
new file mode 100644
index 0000000..845a5fd
Binary files /dev/null and b/spot/src/praktikum3/map/map.pgm differ
diff --git a/spot/src/praktikum3/map/map.yaml b/spot/src/praktikum3/map/map.yaml
new file mode 100644
index 0000000..016923f
--- /dev/null
+++ b/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
\ No newline at end of file
diff --git a/spot/src/praktikum3/package.xml b/spot/src/praktikum3/package.xml
index 901b8f2..0ce1fbb 100644
--- a/spot/src/praktikum3/package.xml
+++ b/spot/src/praktikum3/package.xml
@@ -12,6 +12,10 @@
ament_pep257
python3-pytest
+ nav2_msgs
+ webots_spot_msgs
+ rclpy
+
ament_python
diff --git a/spot/src/praktikum3/praktikum3/p3_nav.py b/spot/src/praktikum3/praktikum3/p3_nav.py
new file mode 100644
index 0000000..cb1f24d
--- /dev/null
+++ b/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()
\ No newline at end of file
diff --git a/spot/src/praktikum3/praktikum3/p3_pushup.py b/spot/src/praktikum3/praktikum3/p3_pushup.py
index 7e84c4d..bc73a5b 100644
--- a/spot/src/praktikum3/praktikum3/p3_pushup.py
+++ b/spot/src/praktikum3/praktikum3/p3_pushup.py
@@ -1,35 +1,43 @@
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()
- rclpy.shutdown()
+ 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__':
- main()
+ main()
\ No newline at end of file
diff --git a/spot/src/praktikum3/setup.py b/spot/src/praktikum3/setup.py
index be1efe9..c8cd4fe 100644
--- a/spot/src/praktikum3/setup.py
+++ b/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'
],
},
-)
+)
\ No newline at end of file