From b77275814a75e0cfe4abd3daa92c4473a559a121 Mon Sep 17 00:00:00 2001 From: Safak Hazinedar Date: Sun, 11 Jan 2026 22:43:10 +0100 Subject: [PATCH] P3 final --- spot/src/praktikum3/map/map.pgm | Bin 0 -> 72576 bytes spot/src/praktikum3/map/map.yaml | 7 ++ spot/src/praktikum3/package.xml | 4 ++ spot/src/praktikum3/praktikum3/p3_nav.py | 71 ++++++++++++++++++++ spot/src/praktikum3/praktikum3/p3_pushup.py | 30 ++++++--- spot/src/praktikum3/setup.py | 12 +++- 6 files changed, 111 insertions(+), 13 deletions(-) create mode 100644 spot/src/praktikum3/map/map.pgm create mode 100644 spot/src/praktikum3/map/map.yaml create mode 100644 spot/src/praktikum3/praktikum3/p3_nav.py diff --git a/spot/src/praktikum3/map/map.pgm b/spot/src/praktikum3/map/map.pgm new file mode 100644 index 0000000000000000000000000000000000000000..845a5fda6aee9efb42231be52bf40b6fb496f1cf GIT binary patch literal 72576 zcmeI5Ym(bI421nVr`RKGvNd~-y|}M5wS0&YC4#RGNQrT)Dl!F5p;=; zVxMB^prT88A?Okv#Xgn}MVKX51YM$|*hkQcFiWm9YqVn0N+Ij?DQ>(eAVoNQ>L?K> z=n@^pKE-#CqASG>=3LE=R#VZ6zH)fG-TwzIc`k9T=ux}X)4mJeC0XKv{*t5MNfsVn)>RWxgKmNgZfMWfrH3tF)Z>26c< zrdc#4|5uwL96ohi;l|el9n%=tCg_21W7nFX6@A4aj%l+-kKvkloeWxYIo=GDY(@sf zw>v>cT$83IXhmO1oazS6_i{^cig2jt5^#bp(NXNv1g+>xafcaKE0+#}&Lq{E72$kF zjYhw}*95KTs~`=fF0^!@n?&ac+eJecTum^c(W1c6c>)SFbivgG6B;cF44o&SKtmT? zO)!Dcg41|2>`p1FOmvlc;mXjXu)Z`8TMw9peGR{XoBt~Fljs}O>i1(bZ=S- zdJ-{$Cg@%Qlg5J|AgJhR`9XgSt{321zit5T8&)*9o;2;Q7{DSRZJ}yeZAf` zviJ81bquX(G(p$do3HpSwei4ABWqOzN-iT?lM%US%M(iJ0oV8ukAwef+b1zoB8CpV4)3JKvb}VMs zrlLDl6)7_zz0(zt6Eq6iV7`(T`0nfp8U<}JThfhXnxJ9OfwW;Aj-wSU`zt}CpuM1- zrgxf!20^2s?L3Rw9y{RB3a@*|6aJ3Ds*iWRba>VW&|Hg(_L^)y`^UQtZ_xB{LrkT- zktgV8#D3uR936^=F}h|Vh6LgL2rN05R zmGh~DSkeT20a?4OgLZW~sT~AeBYWv@0ewi?RSkC{=nKf&WfN#a`dT{(x<>ZW-wOIl znxHQrYnN@HQ%QrM8<(p^?fR>q0h+w@55o4)I_p|IJ{L5Y-&WA(8)|nG^k?|-u6elz zp@BPY@LV2(?V){^Cr!}b*qgZ>^!a-aY_;;w8OlORdhb$n5=e`oC)ksu3AzPuG^R(< zu54_zgPqmy>Gq6xZ_@HV_3w3+w_MVs3T__o4G{pKtz*e8^Z4*+d%hoG3S zyvkzw1Vw)nw4D6B6_nTmKz~5#Fli&`ymsXtP|@JG-Gyi<{D-3f1o zbw(;92wDYWgA>;121pu1mIkc>V3rSpo>!Pj^b94#m&g6_@fBS3>e^zKzK>7SJ^A3$ww&VxGxqJT)V6ypD literal 0 HcmV?d00001 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