Add test park script

[hubacji1/iamcar.git] / test_park.py

diff --git a/test_park.py b/test_park.py
new file mode 100755 (executable)
index 0000000..13a9c46
--- /dev/null
+++ b/test_park.py
@@ -0,0 +1,44 @@
+#!/usr/bin/env python
+from sys import argv, exit
+
+from geometry_msgs.msg import Pose
+from obstacle_msgs.msg import ParkingSlot
+import rospy
+
+from iamcar.srv import Park
+
+def park_cli(init=None, goal=None, slot=None):
+    if not init:
+        exit(1)
+    if not goal:
+        exit(1)
+    if not slot:
+        exit(1)
+    rospy.wait_for_service("Park")
+    try:
+        park_f = rospy.ServiceProxy("Park", Park)
+        res = park_f(init, goal, slot)
+        return res.path
+    except rospy.ServiceException as e:
+        print("Service failed: {}".format(e))
+
+if __name__ == "__main__":
+    init = Pose()
+    init.position.x = 0
+    init.position.y = 0
+    init.position.z = 0
+    init.orientation.x = 0
+    init.orientation.y = 0
+    init.orientation.z = 0
+    init.orientation.w = 1
+    goal = Pose()
+    goal.position.x = 9
+    goal.position.y = 9
+    goal.position.z = 0
+    goal.orientation.x = 0
+    goal.orientation.y = 0
+    goal.orientation.z = 0
+    goal.orientation.w = 1
+    slot = ParkingSlot()
+    print("path len: {}".format(len(park_cli(init, goal, slot))))
+    #print("path: {}".format(park_cli(init, goal, slot)))