test_footsteps.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 import roslib
00004 roslib.load_manifest('naoqi_driver')
00005 import rospy
00006 from rospy.exceptions import ROSException
00007 
00008 
00009 import roslib.rostime
00010 from roslib.rostime import Duration
00011 
00012 
00013 import std_srvs
00014 from std_srvs.srv import Empty
00015 import humanoid_nav_msgs
00016 from humanoid_nav_msgs.msg import StepTarget
00017 from humanoid_nav_msgs.srv import StepTargetService, StepTargetServiceRequest
00018 
00019 def footstep_client():
00020         inhibitWalkSrv = rospy.ServiceProxy("inhibit_walk", std_srvs.srv.Empty)
00021         uninhibitWalkSrv = rospy.ServiceProxy("uninhibit_walk", std_srvs.srv.Empty)             
00022         
00023 
00024         
00025         rospy.loginfo("Waiting for footstep server...")
00026         rospy.wait_for_service('footstep_srv')
00027         client = rospy.ServiceProxy("footstep_srv", humanoid_nav_msgs.srv.StepTargetService)
00028         rospy.loginfo("Done.")
00029         
00030 #       try:
00031         goal = StepTarget()
00032         goal.leg = 0
00033         goal.pose.x = 0.08
00034         goal.pose.theta = 0.3
00035         client(goal)
00036         
00037         goal.leg = 1
00038         goal.pose.theta = 0.3
00039         client(goal)
00040         
00041         goal.leg = 0
00042         goal.pose.x = 0.0
00043         goal.pose.y = -0.16
00044         client(goal)
00045         
00046         goal.leg = 1
00047         goal.pose.x = 0.00
00048         goal.pose.y = 0.088
00049         goal.pose.theta = 0.0
00050         client(goal)
00051         
00052         goal.leg = 0
00053         goal.pose.x = 0.0
00054         goal.pose.y = -0.16
00055         client(goal)
00056         
00057         goal.leg = 1
00058         goal.pose.x = 0.00
00059         goal.pose.y = 0.0
00060         goal.pose.theta = 0.0
00061         client(goal)
00062         
00063         
00064         
00065 if __name__ == '__main__':
00066         try:
00067                 rospy.init_node('footstep_test_client')
00068                 footstep_client()
00069                 
00070         except rospy.ROSInterruptException:
00071                 print "program interrupted before completion"
00072                 


nao_apps
Author(s):
autogenerated on Sat Jun 8 2019 20:38:35