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


nao_driver
Author(s): Armin Hornung, Armin Hornung, Stefan Osswald, Daniel Maier, Miguel Sarabia, Séverin Lemaignan
autogenerated on Thu Oct 30 2014 09:20:04