nao_footsteps.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # SVN $HeadURL$
00004 # SVN $Id$
00005 
00006 
00007 #
00008 # ROS node to control Nao's footsteps (testing for NaoQI 1.12)
00009 #
00010 # Copyright 2012 Armin Hornung and Johannes Garimort, University of Freiburg
00011 # http://www.ros.org/wiki/nao_driver
00012 #
00013 # Redistribution and use in source and binary forms, with or without
00014 # modification, are permitted provided that the following conditions are met:
00015 #
00016 #    # Redistributions of source code must retain the above copyright
00017 #       notice, this list of conditions and the following disclaimer.
00018 #    # Redistributions in binary form must reproduce the above copyright
00019 #       notice, this list of conditions and the following disclaimer in the
00020 #       documentation and/or other materials provided with the distribution.
00021 #    # Neither the name of the University of Freiburg nor the names of its
00022 #       contributors may be used to endorse or promote products derived from
00023 #       this software without specific prior written permission.
00024 #
00025 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00026 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00027 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00028 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00029 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00030 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00031 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00032 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00033 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00034 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035 # POSSIBILITY OF SUCH DAMAGE.
00036 #
00037 
00038 import roslib
00039 roslib.load_manifest('nao_driver')
00040 import rospy
00041 import time
00042 
00043 from nao_driver import *
00044 
00045 import math
00046 from math import fabs
00047 
00048 import actionlib
00049 
00050 from humanoid_nav_msgs.msg import *
00051 from humanoid_nav_msgs.srv import StepTargetService, StepTargetServiceResponse
00052 from humanoid_nav_msgs.srv import ClipFootstep, ClipFootstepResponse
00053 
00054 from start_walk_pose import startWalkPose
00055 from nao_footstep_clipping import clip_footstep_tuple
00056 
00057 
00058 LEG_LEFT = "LLeg"
00059 LEG_RIGHT = "RLeg"
00060 FLOAT_CMP_THR = 0.000001
00061 STEP_TIME = 0.5
00062 
00063 
00064 def float_equ(a, b):
00065     return abs(a - b) <= FLOAT_CMP_THR
00066 
00067 
00068 # redefine StepTarget by inheritance
00069 class StepTarget(StepTarget):
00070     def __init__(self, x=0.0, y=0.0, theta=0.0, leg=0.0):
00071         super(StepTarget, self).__init__()
00072         self.pose.x = round(x, 4)
00073         self.pose.y = round(y, 4)
00074         self.pose.theta = round(theta, 4)
00075         self.leg = leg
00076 
00077     def __eq__(self, a):
00078         return (float_equ(self.pose.x, a.pose.x) and
00079                 float_equ(self.pose.y, a.pose.y) and
00080                 float_equ(self.pose.theta, a.pose.theta) and
00081                 self.leg == a.leg)
00082 
00083     def __ne__(self, a):
00084         return not (self == a)
00085 
00086     def __str__(self):
00087         return "(%f, %f, %f, %i)" % (self.pose.x, self.pose.y, self.pose.theta,
00088                                      self.leg)
00089 
00090     def __repr__(self):
00091         return self.__str__()
00092 
00093 
00094 class NaoFootsteps(NaoNode):
00095     def __init__(self):
00096         NaoNode.__init__(self)
00097 
00098         # ROS initialization:
00099         rospy.init_node('nao_footsteps')
00100 
00101         self.connectNaoQi()
00102 
00103         # initial stiffness (defaults to 0 so it doesn't strain the robot when
00104         # no teleoperation is running)
00105         # set to 1.0 if you want to control the robot immediately
00106         initStiffness = rospy.get_param('~init_stiffness', 0.0)
00107 
00108         # TODO: parameterize
00109         if initStiffness > 0.0 and initStiffness <= 1.0:
00110             self.motionProxy.stiffnessInterpolation('Body', initStiffness, 0.5)
00111 
00112         # last: ROS subscriptions (after all vars are initialized)
00113         rospy.Subscriber("footstep", StepTarget, self.handleStep, queue_size=50)
00114 
00115         # ROS services (blocking functions)
00116         self.stepToSrv = rospy.Service("footstep_srv", StepTargetService,
00117                                        self.handleStepSrv)
00118         self.clipSrv = rospy.Service("clip_footstep_srv", ClipFootstep,
00119                                      self.handleClipSrv)
00120 
00121         # Initialize action server
00122         self.actionServer = actionlib.SimpleActionServer(
00123             "footsteps_execution",
00124             ExecFootstepsAction,
00125             execute_cb=self.footstepsExecutionCallback,
00126             auto_start=False)
00127         self.actionServer.start()
00128 
00129         rospy.loginfo("nao_footsteps initialized")
00130 
00131     def connectNaoQi(self):
00132         """(re-) connect to NaoQI"""
00133         rospy.loginfo("Connecting to NaoQi at %s:%d", self.pip, self.pport)
00134 
00135         self.motionProxy = self.getProxy("ALMotion")
00136         if self.motionProxy is None:
00137             exit(1)
00138 
00139 
00140     def stopWalk(self):
00141         """
00142         Stops the current walking bahavior and blocks until the clearing is
00143         complete.
00144         """
00145         try:
00146             self.motionProxy.setWalkTargetVelocity(0.0, 0.0, 0.0, self.stepFrequency)
00147             self.motionProxy.waitUntilWalkIsFinished()
00148 
00149         except RuntimeError,e:
00150             print "An error has been caught"
00151             print e
00152             return False
00153 
00154         return True
00155 
00156 
00157     def handleStep(self, data):
00158         rospy.loginfo("Step leg: %d; target: %f %f %f", data.leg, data.pose.x,
00159                 data.pose.y, data.pose.theta)
00160         try:
00161             if data.leg == StepTarget.right:
00162                 leg = [LEG_RIGHT]
00163             elif data.leg == StepTarget.left:
00164                 leg = [LEG_LEFT]
00165             else:
00166                 rospy.logerr("Received a wrong leg constant: %d, ignoring step",
00167                              " command", data.leg)
00168                 return
00169 
00170             footStep = [[data.pose.x, data.pose.y, data.pose.theta]]
00171             timeList = [STEP_TIME]
00172             self.motionProxy.setFootSteps(leg, footStep, timeList, False)
00173             time.sleep(0.1)
00174             print self.motionProxy.getFootSteps()
00175             self.motionProxy.waitUntilWalkIsFinished()
00176 
00177             return True
00178         except RuntimeError, e:
00179             rospy.logerr("Exception caught in handleStep:\n%s", e)
00180             return False
00181 
00182     def handleStepSrv(self, req):
00183         if self.handleStep(req.step):
00184             return StepTargetServiceResponse()
00185         else:
00186             return None
00187 
00188     def handleClipping(self, step):
00189         is_left_support = step.leg != StepTarget.left
00190         unclipped_step = (step.pose.x, step.pose.y, step.pose.theta)
00191         step.pose.x, step.pose.y, step.pose.theta = clip_footstep_tuple(
00192             unclipped_step, is_left_support)
00193         return step
00194 
00195     def handleClipSrv(self, req):
00196         resp = ClipFootstepResponse()
00197         resp.step = self.handleClipping(req.step)
00198         return resp
00199 
00200     def footstepsExecutionCallback(self, goal):
00201         def update_feedback(feedback, executed_footsteps):
00202             # check if an footstep has been performed
00203             if not len(executed_footsteps):
00204                 return
00205             # use the last footstep in the list since this might be the new one
00206             # (NOTE: if one step is missed here, increase the feedback rate)
00207             leg, time, (x, y, theta) = executed_footsteps[-1]
00208             # check if footstep information is up-to-date
00209             if not float_equ(time, STEP_TIME):
00210                 return
00211             leg = (StepTarget.right if leg == LEG_RIGHT else
00212                    StepTarget.left)
00213             step = StepTarget(x, y, theta, leg)
00214             # add the footstep only if it is a new one
00215             try:
00216                 if feedback.executed_footsteps[-1] == step:
00217                     return
00218             except IndexError:
00219                 pass
00220             feedback.executed_footsteps.append(step)
00221 
00222         legs = []
00223         steps = []
00224         time_list = []
00225         for step in goal.footsteps:
00226             if step.leg == StepTarget.right:
00227                 legs.append(LEG_RIGHT)
00228             elif step.leg == StepTarget.left:
00229                 legs.append(LEG_LEFT)
00230             else:
00231                 rospy.logerr("Received a wrong leg constant: %d, ignoring step "
00232                              "command", step.leg)
00233                 return
00234             steps.append([round(step.pose.x, 4),
00235                           round(step.pose.y, 4),
00236                           round(step.pose.theta, 4)])
00237             try:
00238                 time_list.append(time_list[-1] + STEP_TIME)
00239             except IndexError:
00240                 time_list.append(STEP_TIME)
00241 
00242         rospy.loginfo("Start executing footsteps %s",
00243                       [[x, y, theta, leg] for (x, y, theta), leg in
00244                        zip(steps, legs)])
00245 
00246         feedback = ExecFootstepsFeedback()
00247         result = ExecFootstepsResult()
00248         success = True
00249         self.motionProxy.setFootSteps(legs, steps, time_list, True)
00250         while self.motionProxy.walkIsActive():
00251             # handle preempt requests
00252             if self.actionServer.is_preempt_requested():
00253                 self.motionProxy.stopWalk()
00254                 self.actionServer.set_preempted()
00255                 rospy.loginfo("Preempting footstep execution");
00256                 success = False
00257                 break
00258 
00259             # get execution information from the robot and update the feedback
00260             (_, executed_footsteps, _) = self.motionProxy.getFootSteps()
00261             update_feedback(feedback, executed_footsteps)
00262             self.actionServer.publish_feedback(feedback)
00263 
00264             rospy.Rate(goal.feedback_frequency).sleep()
00265 
00266         if success:
00267             result.executed_footsteps = feedback.executed_footsteps
00268             self.actionServer.set_succeeded(result)
00269 
00270 
00271 if __name__ == '__main__':
00272 
00273     walker = NaoFootsteps()
00274     rospy.loginfo("nao_footsteps running...")
00275     rospy.spin()
00276     rospy.loginfo("nao_footsteps stopping...")
00277     walker.stopWalk()
00278 
00279     rospy.loginfo("nao_footsteps stopped.")
00280     exit(0)
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


nao_driver
Author(s): Armin Hornung, Stefan Osswald, Daniel Maier
autogenerated on Tue Oct 15 2013 10:06:23