nao_walker.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 #
00004 # ROS node to control Nao's walking engine (omniwalk and footsteps)
00005 # This code is currently compatible to NaoQI version 1.6 or newer (latest
00006 # tested: 1.12)
00007 #
00008 # Copyright 2009-2011 Armin Hornung & Stefan Osswald, University of Freiburg
00009 # http://www.ros.org/wiki/nao
00010 #
00011 # Redistribution and use in source and binary forms, with or without
00012 # modification, are permitted provided that the following conditions are met:
00013 #
00014 #    # Redistributions of source code must retain the above copyright
00015 #       notice, this list of conditions and the following disclaimer.
00016 #    # Redistributions in binary form must reproduce the above copyright
00017 #       notice, this list of conditions and the following disclaimer in the
00018 #       documentation and/or other materials provided with the distribution.
00019 #    # Neither the name of the University of Freiburg nor the names of its
00020 #       contributors may be used to endorse or promote products derived from
00021 #       this software without specific prior written permission.
00022 #
00023 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00024 # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00025 # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00026 # ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00027 # LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00028 # CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00029 # SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00030 # INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00031 # CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00032 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 # POSSIBILITY OF SUCH DAMAGE.
00034 #
00035 
00036 import rospy
00037 
00038 from naoqi_driver.naoqi_node import NaoqiNode
00039 
00040 from std_msgs.msg import String
00041 from geometry_msgs.msg import Twist
00042 from geometry_msgs.msg import Pose2D
00043 
00044 from std_srvs.srv import Empty, EmptyResponse
00045 from naoqi_bridge_msgs.srv import CmdPoseService, CmdVelService, CmdPoseServiceResponse, CmdVelServiceResponse, SetArmsEnabled, SetArmsEnabledResponse
00046 from humanoid_nav_msgs.msg import StepTarget
00047 from humanoid_nav_msgs.srv import StepTargetService, StepTargetServiceResponse
00048 
00049 from nao_apps import startWalkPose
00050 
00051 class NaoWalker(NaoqiNode):
00052     def __init__(self):
00053         NaoqiNode.__init__(self, 'nao_walker')
00054 
00055         self.connectNaoQi()
00056 
00057         # walking pattern params:
00058         self.stepFrequency = rospy.get_param('~step_frequency', 0.5)
00059 
00060         self.useStartWalkPose = rospy.get_param('~use_walk_pose', False)
00061         self.needsStartWalkPose = True
00062 
00063         # other params
00064         self.maxHeadSpeed = rospy.get_param('~max_head_speed', 0.2)
00065         # initial stiffness (defaults to 0 so it doesn't strain the robot when no teleoperation is running)
00066         # set to 1.0 if you want to control the robot immediately
00067         initStiffness = rospy.get_param('~init_stiffness', 0.0)
00068 
00069         self.useFootGaitConfig = rospy.get_param('~use_foot_gait_config', False)
00070         rospy.loginfo("useFootGaitConfig = %d" % self.useFootGaitConfig)
00071         if self.useFootGaitConfig:
00072             self.footGaitConfig = rospy.get_param('~foot_gait_config', self.motionProxy.getFootGaitConfig("Default"))
00073         else:
00074             self.footGaitConfig = self.motionProxy.getFootGaitConfig("Default")
00075 
00076         # TODO: parameterize
00077         if initStiffness > 0.0 and initStiffness <= 1.0:
00078             self.motionProxy.stiffnessInterpolation('Body', initStiffness, 0.5)
00079 
00080         try:
00081             enableFootContactProtection = rospy.get_param('~enable_foot_contact_protection')
00082             self.motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", enableFootContactProtection]])
00083             if enableFootContactProtection:
00084                 rospy.loginfo("Enabled foot contact protection")
00085             else:
00086                 rospy.loginfo("Disabled foot contact protection")
00087         except KeyError:
00088             # do not change foot contact protection
00089             pass
00090 
00091         # last: ROS subscriptions (after all vars are initialized)
00092         rospy.Subscriber("cmd_vel", Twist, self.handleCmdVel, queue_size=1)
00093         rospy.Subscriber("cmd_pose", Pose2D, self.handleTargetPose, queue_size=1)
00094         rospy.Subscriber("cmd_step", StepTarget, self.handleStep, queue_size=50)
00095 
00096         # Create ROS publisher for speech
00097         self.pub = rospy.Publisher("speech", String, latch = True)
00098 
00099         # ROS services (blocking functions)
00100         self.cmdPoseSrv = rospy.Service("cmd_pose_srv", CmdPoseService, self.handleTargetPoseService)
00101         self.cmdVelSrv = rospy.Service("cmd_vel_srv", CmdVelService, self.handleCmdVelService)
00102         self.stepToSrv = rospy.Service("cmd_step_srv", StepTargetService, self.handleStepSrv)
00103         self.stopWalkSrv = rospy.Service("stop_walk_srv", Empty, self.handleStopWalkSrv)
00104         self.needsStartWalkPoseSrv = rospy.Service("needs_start_walk_pose_srv", Empty, self.handleNeedsStartWalkPoseSrv)
00105         self.readFootGaitConfigSrv = rospy.Service("read_foot_gait_config_srv", Empty, self.handleReadFootGaitConfigSrv)
00106         self.setArmsEnabledSrv = rospy.Service("enable_arms_walking_srv", SetArmsEnabled, self.handleSetArmsEnabledSrv)
00107 
00108         self.say("Walker online")
00109 
00110         rospy.loginfo("nao_walker initialized")
00111 
00112     def connectNaoQi(self):
00113         '''(re-) connect to NaoQI'''
00114         rospy.loginfo("Connecting to NaoQi at %s:%d", self.pip, self.pport)
00115 
00116         self.motionProxy = self.get_proxy("ALMotion")
00117         if self.motionProxy is None:
00118             exit(1)
00119 
00120     def stopWalk(self):
00121         """ Stops the current walking bahavior and blocks until the clearing is complete. """
00122         try:
00123             self.motionProxy.setWalkTargetVelocity(0.0, 0.0, 0.0, self.stepFrequency)
00124             self.motionProxy.waitUntilWalkIsFinished()
00125 
00126 
00127         except RuntimeError,e:
00128             print "An error has been caught"
00129             print e
00130             return False
00131 
00132         return True
00133 
00134 
00135     def say(self, text):
00136         self.pub.publish(text)
00137 
00138     def handleCmdVel(self, data):
00139         rospy.logdebug("Walk cmd_vel: %f %f %f, frequency %f", data.linear.x, data.linear.y, data.angular.z, self.stepFrequency)
00140         if data.linear.x != 0 or data.linear.y != 0 or data.angular.z != 0:
00141             self.gotoStartWalkPose()
00142         try:
00143             eps = 1e-3 # maybe 0,0,0 is a special command in motionProxy...
00144             if abs(data.linear.x)<eps and abs(data.linear.y)<eps and abs(data.angular.z)<eps:
00145                 self.motionProxy.setWalkTargetVelocity(0,0,0,0.5)
00146             else:
00147                 self.motionProxy.setWalkTargetVelocity(data.linear.x, data.linear.y, data.angular.z, self.stepFrequency, self.footGaitConfig)
00148         except RuntimeError,e:
00149             # We have to assume there's no NaoQI running anymore => exit!
00150             rospy.logerr("Exception caught in handleCmdVel:\n%s", e)
00151             rospy.signal_shutdown("No NaoQI available anymore")
00152 
00153 
00154 
00155     def handleCmdVelService(self, req):
00156         self.handleCmdVel(req.twist)
00157         return CmdVelServiceResponse()
00158 
00159     def handleTargetPose(self, data, post=True):
00160         """handles cmd_pose requests, walks to (x,y,theta) in robot coordinate system"""
00161 
00162         rospy.logdebug("Walk target_pose: %f %f %f", data.x,
00163                 data.y, data.theta)
00164 
00165         self.gotoStartWalkPose()
00166 
00167         try:
00168             if post:
00169                 self.motionProxy.post.walkTo(data.x, data.y, data.theta, self.footGaitConfig)
00170             else:
00171                 self.motionProxy.walkTo(data.x, data.y, data.theta, self.footGaitConfig)
00172             return True
00173         except RuntimeError,e:
00174             rospy.logerr("Exception caught in handleTargetPose:\n%s", e)
00175             return False
00176 
00177 
00178     def handleStep(self, data):
00179         rospy.logdebug("Step leg: %d; target: %f %f %f", data.leg, data.pose.x,
00180                 data.pose.y, data.pose.theta)
00181         try:
00182             if data.leg == StepTarget.right:
00183                 leg = "RLeg"
00184             elif data.leg == StepTarget.left:
00185                 leg = "LLeg"
00186             else:
00187                 rospy.logerr("Received a wrong leg constant: %d, ignoring step command", data.leg)
00188                 return
00189             self.motionProxy.stepTo(leg, data.pose.x, data.pose.y, data.pose.theta)
00190             return True
00191         except RuntimeError, e:
00192             rospy.logerr("Exception caught in handleStep:\n%s", e)
00193             return False
00194 
00195     def handleStepSrv(self, req):
00196         if self.handleStep(req.step):
00197             return StepTargetServiceResponse()
00198         else:
00199             return None
00200 
00201     def handleTargetPoseService(self, req):
00202         """ do NOT use post"""
00203         if self.handleTargetPose(req.pose, False):
00204             return CmdPoseServiceResponse()
00205         else:
00206             return None
00207 
00208     def handleStopWalkSrv(self, req):
00209         if self.stopWalk():
00210             return EmptyResponse()
00211         else:
00212             return None
00213 
00214     def gotoStartWalkPose(self):
00215         if self.useStartWalkPose and self.needsStartWalkPose:
00216             startWalkPose(self.motionProxy)
00217             self.needsStartWalkPose = False
00218 
00219     def handleNeedsStartWalkPoseSrv(self, data):
00220         self.needsStartWalkPose = True
00221         return EmptyResponse()
00222 
00223     def handleReadFootGaitConfigSrv(self, data):
00224         self.useFootGaitConfig = rospy.get_param('~use_foot_gait_config', False)
00225         rospy.loginfo("useFootGaitConfig = %d" % self.useFootGaitConfig)
00226         if self.useFootGaitConfig:
00227             self.footGaitConfig = rospy.get_param('~foot_gait_config', self.motionProxy.getFootGaitConfig("Default"))
00228         else:
00229             self.footGaitConfig = self.motionProxy.getFootGaitConfig("Default")
00230         return EmptyResponse()
00231 
00232     def handleSetArmsEnabledSrv(self, req):
00233         self.motionProxy.setWalkArmsEnable(req.left_arm, req.right_arm)
00234         rospy.loginfo("Arms enabled during walk: left(%s) right(%s)" % (req.left_arm, req.right_arm))
00235         return SetArmsEnabledResponse()
00236 
00237 
00238 if __name__ == '__main__':
00239     walker = NaoWalker()
00240     rospy.loginfo("nao_walker running...")
00241     rospy.spin()
00242     rospy.loginfo("nao_walker stopping...")
00243     walker.stopWalk()
00244 
00245     rospy.loginfo("nao_walker stopped.")
00246     exit(0)


nao_apps
Author(s):
autogenerated on Thu Aug 27 2015 14:02:19