plan_footsteps.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 #
00004 # A simple service client calling a running footstep_planner node.
00005 # Using this program you can request footstep plans from the command line.
00006 #
00007 # Author: Armin Hornung, University of Freiburg
00008 #
00009 # This is program is part of the ROS footstep planner:
00010 # http://www.ros.org/wiki/footstep_planner
00011 # License: GPL 3
00012 #
00013 
00014 import roslib
00015 from humanoid_nav_msgs.msg._StepTarget import StepTarget
00016 roslib.load_manifest('footstep_planner')
00017 import rospy
00018 
00019 from humanoid_nav_msgs.srv import *
00020 from geometry_msgs.msg import Pose2D
00021 
00022 import sys
00023 
00024 
00025 if __name__ == '__main__':
00026     if len(sys.argv) != 7 and len(sys.argv) != 13:
00027       sys.exit('\nUSAGE: %s <start> <goal>\n  where <start> and <goal> consist of "x y theta" in world coordinates\n\n' % sys.argv[0])
00028 
00029     rospy.init_node('plan_footsteps')
00030     
00031     if len(sys.argv) ==7:
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     else:
00048       planSrv = rospy.ServiceProxy("plan_footsteps_feet", PlanFootstepsBetweenFeet)
00049       start_left = StepTarget()
00050       start_right = StepTarget()
00051       goal_left = StepTarget()
00052       goal_right = StepTarget()
00053       
00054       start_left.pose.x = float(sys.argv[1])
00055       start_left.pose.y = float(sys.argv[2])
00056       start_left.pose.theta = float(sys.argv[3])
00057       start_right.pose.x = float(sys.argv[4])
00058       start_right.pose.y = float(sys.argv[5])
00059       start_right.pose.theta = float(sys.argv[6])
00060       
00061       goal_left.pose.x = float(sys.argv[7])
00062       goal_left.pose.y = float(sys.argv[8])
00063       goal_left.pose.theta = float(sys.argv[9])
00064       goal_right.pose.x = float(sys.argv[10])
00065       goal_right.pose.y = float(sys.argv[11])
00066       goal_right.pose.theta = float(sys.argv[12])
00067       
00068       resp = planSrv(start_left, start_right, goal_left, goal_right);
00069 
00070     if resp.result == True:
00071         rospy.loginfo("Planning succeeded with %d steps, path costs: %f" % (len(resp.footsteps), resp.costs))
00072         print "Footsteps:"
00073         for f in resp.footsteps:
00074             print ("L" if (f.leg == StepTarget.left) else "R") + " [%f %f %f]" % (f.pose.x, f.pose.y, f.pose.theta)
00075     else:
00076         rospy.logerr("Service call failed")
00077 
00078     exit(0)


footstep_planner
Author(s): Johannes Garimort, Armin Hornung
autogenerated on Mon Oct 6 2014 00:41:41