def interact(self, args):
rospy.wait_for_service('voice_control')
srv = rospy.ServiceProxy('voice_control', voice_control)
try:
success = srv()
print 'Voice control called: ' + str(success)
except:
print 'No response from voice control'
def serve(self, args):
coffee = args[6:]
print 'Serving coffee "' + coffee + '"'
rospy.wait_for_service('coffee_machine')
srv = rospy.ServiceProxy('coffee_machine', coffee_machine)
try:
resp = srv(coffee)
print resp
except rospy.ServiceExecution, e:
print 'Service call failed: ' + e
def moveIt(self):
rate = rospy.Rate(self.frequency)
timeout = 0.1
originFrame = 'torso_1'
destFrame = 'base_link'
while not rospy.is_shutdown():
while self.pause:
rate.sleep()
id = self.pickPersonToFollow()
originFrame = 'torso_' + str(id)
t0 = rospy.Time().now()
while rospy.Time.now() - t0 < rospy.Duration(3, 0):
while self.pause:
rate.sleep()
self.linearSpeed = 0.0
self.angularSpeed = 0.0
#rospy.loginfo(originFrame)
now = rospy.Time(0)
try:
self.listener.waitForTransform(destFrame, originFrame, now, rospy.Duration(timeout))
(trans, rot) = self.listener.lookupTransform(destFrame, originFrame, now)
except:
#traceback.print_exc(file=sys.stdout)
rate.sleep()
continue
tInput = PointStamped()
tOutput = PointStamped()
tInput.header.frame_id = originFrame
tInput.header.stamp = rospy.Time(0)
tInput.point.x = 0.0
tInput.point.y = 0.0
tInput.point.z = 0.0
tOutput = self.listener.transformPoint(destFrame, tInput)
distance = sqrt(pow(tOutput.point.x, 2) + pow(tOutput.point.y, 2))
angle = atan2(tOutput.point.y, tOutput.point.x)
self.computeSpeed(distance, angle)
rospy.loginfo('Distance: ' + str(distance))
rospy.loginfo('Angle: ' + str(angle))
rospy.loginfo('------------------------')
self.publish(self.linearSpeed, self.angularSpeed)
if self.linearSpeed == 0 and self.angularSpeed == 0:
#if we're not moving anymore, call voice-control service
rospy.wait_for_service('voice_control')
srv = rospy.ServiceProxy('voice_control', voice_control)
try:
success = srv()
rospy.loginfo('Called voice-control, success: ' + str(success))
except:
rospy.logerr('Voice control server failed to respond, call Rich and insult him')
if self.restart:
break
rate.sleep()
return 0
