00001
00002
00003 import math
00004 import time
00005 import naoqi
00006 import motion
00007 from naoqi import ALProxy
00008 import roslib
00009 roslib.load_manifest('nao_driver')
00010 import rospy
00011
00012 from init_pose import initPose
00013 from scan_pose_bending import scanPoseBending
00014 from camera_walk_pose import cameraWalkPose
00015 from std_msgs.msg import Bool
00016
00017 def head_scan(motionProxy,ttsProxy,headPitchStart=26.0,headPitchEnd=-10.0,angularResolution=0.5,laserScanFreq=10,use_scan_pose=False):
00018 scanInfoPub = rospy.Publisher("scan_info", Bool)
00019 scanInfo = Bool()
00020
00021
00022 headPitchStart = headPitchStart * math.pi/180.0
00023 headPitchEnd = headPitchEnd * math.pi/180.0
00024 degreePerSecond = angularResolution* laserScanFreq*0.33
00025 radiansPerSecond = degreePerSecond*math.pi/180.0
00026 radiansPerSecondFast = 0.35
00027
00028
00029 head_pitch_before_scan = motionProxy.getAngles("HeadPitch",True)[0]
00030 head_pitch_after_scan = head_pitch_before_scan
00031 if use_scan_pose:
00032 initPose(motionProxy)
00033 scanPoseBending(motionProxy,head_pitch_before_scan,0.0,2)
00034
00035
00036 t1 = abs(head_pitch_before_scan - headPitchStart)/radiansPerSecondFast
00037 t2 = abs(headPitchStart - headPitchEnd)/radiansPerSecond
00038 t3 = abs(headPitchEnd - head_pitch_after_scan)/radiansPerSecondFast
00039
00040 motionProxy.angleInterpolation('HeadPitch', [headPitchStart], [t1], True)
00041 rospy.sleep(0.5)
00042 if (not ttsProxy is None):
00043 try:
00044 ttsid = ttsProxy.post.say("Start scan")
00045
00046 except RuntimeError,e:
00047 rospy.logerr("Exception caught:\n%s", e)
00048 startTime = rospy.Time.now().to_sec();
00049 scanInfo.data = True
00050 scanInfoPub.publish(scanInfo)
00051
00052 motionProxy.angleInterpolation('HeadPitch', [headPitchEnd], [t2], True)
00053
00054 stopTime = rospy.Time.now().to_sec();
00055 scanInfo.data = False
00056 scanInfoPub.publish(scanInfo)
00057
00058 if (not ttsProxy is None):
00059 try:
00060 ttsid = ttsProxy.post.say("Scan complete.")
00061
00062 except RuntimeError,e:
00063 rospy.logerr("Exception caught:\n%s", e)
00064 rospy.sleep(0.5)
00065
00066
00067
00068 if use_scan_pose:
00069 cameraWalkPose(motionProxy, head_pitch_after_scan, 0.0)
00070
00071 if __name__ == '__main__':
00072
00073 from optparse import OptionParser
00074 rospy.init_node('headscan')
00075
00076 parser = OptionParser()
00077 parser.add_option("--pip", dest="pip", default="127.0.0.1",
00078 help="IP/hostname of parent broker. Default is 127.0.0.1.", metavar="IP")
00079 parser.add_option("--pport", dest="pport", default=9559,
00080 help="port of parent broker. Default is 9559.", metavar="PORT")
00081
00082 parser.add_option("--s", dest="headpitchstart", default=27.0,
00083 help="start head pitch", metavar="START")
00084 parser.add_option("--e", dest="headpitchend", default=-10.0,
00085 help="end head pitch (negative angles are towards sky)",
00086 metavar="END")
00087 parser.add_option("--speed", dest="headpitchspeed", default=10,
00088 help="head pitch speed(degree per second)", metavar="SPEED")
00089 parser.add_option("--pose", dest="usescanpose", default=False, help="use scan pose", metavar="BOOL")
00090
00091 (options, args) = parser.parse_args()
00092
00093 headPitchStart = float(options.headpitchstart )
00094 headPitchEnd = float(options.headpitchend)
00095
00096
00097 pitch_speed = float(options.headpitchspeed)
00098 useScanPose = bool(options.usescanpose)
00099 angularResolution = 1
00100 laserScanFreq = pitch_speed
00101 try:
00102 motionProxy = ALProxy("ALMotion",options.pip,int(options.pport))
00103 ttsProxy = ALProxy("ALTextToSpeech",options.pip,int(options.pport))
00104 except RuntimeError,e:
00105 print("Error creating Proxy to motion, exiting...")
00106 print e
00107 exit(1)
00108
00109 head_scan(motionProxy,headPitchStart,headPitchEnd,angularResolution,laserScanFreq,useScanPose)
00110 print("Scan completed...")
00111 exit(0)
00112
00113
