$search
00001 #!/usr/bin/env python 00002 00003 # SVN $HeadURL: http://alufr-ros-pkg.googlecode.com/svn/trunk/humanoid_stacks/humanoid_navigation/footstep_planner/scripts/plan_footsteps.py $ 00004 # SVN $Id: plan_footsteps.py 3810 2013-01-20 18:57:24Z garimort@informatik.uni-freiburg.de $ 00005 00006 # 00007 # A simple service client calling a running footstep_planner node. 00008 # Using this program you can request footstep plans from the command line. 00009 # 00010 # Author: Armin Hornung, University of Freiburg 00011 # 00012 # This is program is part of the ROS footstep planner: 00013 # http://www.ros.org/wiki/footstep_planner 00014 # License: GPL 3 00015 # 00016 00017 import roslib 00018 roslib.load_manifest('footstep_planner') 00019 import rospy 00020 00021 from humanoid_nav_msgs.srv import * 00022 from geometry_msgs.msg import Pose2D 00023 00024 import sys 00025 00026 00027 if __name__ == '__main__': 00028 if len(sys.argv) != 7: 00029 sys.exit('\nUSAGE: %s <start> <goal>\n where <start> and <goal> consist of "x y theta" in world coordinates\n\n' % sys.argv[0]) 00030 00031 rospy.init_node('plan_footsteps') 00032 planSrv = rospy.ServiceProxy("plan_footsteps", PlanFootsteps) 00033 start = Pose2D() 00034 goal = Pose2D() 00035 00036 start.x = float(sys.argv[1]) 00037 start.y = float(sys.argv[2]) 00038 start.theta = float(sys.argv[3]) 00039 00040 goal.x = float(sys.argv[4]) 00041 goal.y = float(sys.argv[5]) 00042 goal.theta = float(sys.argv[6]) 00043 00044 rospy.loginfo("Calling footstep planner service from (%f %f %f) to (%f %f %f)...", 00045 start.x, start.y, start.theta, goal.x, goal.y, goal.theta) 00046 resp = planSrv(start, goal) 00047 00048 if resp.result == True: 00049 rospy.loginfo("Planning succeeded with %d steps, path costs: %f" % (len(resp.footsteps), resp.costs)) 00050 else: 00051 rospy.logerr("Service call failed") 00052 00053 exit(0)