test_park.py

   1 #!/usr/bin/env python
   2 from sys import argv, exit
   3
   4 from geometry_msgs.msg import Pose
   5 from obstacle_msgs.msg import ParkingSlot
   6 import rospy
   7
   8 from iamcar.srv import Park
   9
  10 def park_cli(init=None, goal=None, slot=None):
  11     if not init:
  12         exit(1)
  13     if not goal:
  14         exit(1)
  15     if not slot:
  16         exit(1)
  17     rospy.wait_for_service("Park")
  18     try:
  19         park_f = rospy.ServiceProxy("Park", Park)
  20         res = park_f(init, goal, slot)
  21         return res.path
  22     except rospy.ServiceException as e:
  23         print("Service failed: {}".format(e))
  24
  25 if __name__ == "__main__":
  26     init = Pose()
  27     init.position.x = 0
  28     init.position.y = 0
  29     init.position.z = 0
  30     init.orientation.x = 0
  31     init.orientation.y = 0
  32     init.orientation.z = 0
  33     init.orientation.w = 1
  34     goal = Pose()
  35     goal.position.x = 9
  36     goal.position.y = 9
  37     goal.position.z = 0
  38     goal.orientation.x = 0
  39     goal.orientation.y = 0
  40     goal.orientation.z = 0
  41     goal.orientation.w = 1
  42     slot = ParkingSlot()
  43     print("path len: {}".format(len(park_cli(init, goal, slot))))
  44     #print("path: {}".format(park_cli(init, goal, slot)))