def __init__(self, debug=False, online = True, robot_frame="odom_combined"):
self._tf = TransformListener()
self._online = online
# self.snd_handle = SoundClient()
if self._online:
#self._interface = ROSpeexInterface()
#self._interface.init()
self._speech_client = SpeechSynthesisClient_NICT()
else:
self.snd_handle = SoundClient()
rospy.sleep(1)
self.say('Hello world!')
rospy.sleep(1)
self._debug = debug
self._robot_frame = robot_frame
self._point_sub = rospy.Subscriber('nearest_face', PointStamped, self.face_cb)
self._head_action_cl = actionlib.SimpleActionClient('/head_traj_controller/point_head_action', pr2_controllers_msgs.msg.PointHeadAction)
self._torso_action_cl = actionlib.SimpleActionClient('/torso_controller/position_joint_action', pr2_controllers_msgs.msg.SingleJointPositionAction)
self._left_arm = MoveGroupCommander("left_arm")
self._right_arm = MoveGroupCommander("right_arm")
print "r.f.: " + self._left_arm.get_pose_reference_frame()
self.face = None
# self.face_from = rospy.Time(0)
self.face_last_dist = 0
self.last_invited_at = rospy.Time(0)
self._person_prob_left = 0
self.l_home_pose = [0.283, 0.295, 0.537, -1.646, 0.468, -1.735]
self.l_wave_1 = [-0.1, 0.6, 1.15, -1.7, -0.97, -1.6]
self.l_wave_2 = [-0.1, 0.6, 1.15, 1.7, -0.97, 1.6]
self.r_home_pose = [0.124, -0.481, 0.439, -1.548, 0.36, -0.035]
self.r_advert = [0.521, -0.508, 0.845, -1.548, 0.36, -0.035]
self.no_face_random_delay = None
self._initialized = False
self._timer = rospy.Timer(rospy.Duration(1.0), self.timer)
self._move_buff = Queue.Queue()
self._head_buff = Queue.Queue()
self._move_thread = threading.Thread(target=self.movements)
self._move_thread.daemon = True
self._move_thread.start()
self._head_thread = threading.Thread(target=self.head)
self._head_thread.daemon = True
self._head_thread.start()
self.new_face = False
self.face_last_dist = 0.0
self.face_counter = 0
self.actions = [self.stretchingAction,
self.introduceAction,
self.waveHandAction,
self.lookAroundAction,
self.lookAroundAction,
self.lookAroundAction,
self.advertAction,
self.numberOfFacesAction]
self.goodbye_strings = ["Thanks for stopping by.",
"Enjoy the event.",
"See you later!",
"Have a nice day!"]
self.invite_strings = ["Hello. It's nice to see you.",
"Come here and take some flyer.",
"I hope you are enjoying the event."]
rospy.loginfo("Ready")
class PR2Greeter:
def __init__(self, debug=False, online = True, robot_frame="odom_combined"):
self._tf = TransformListener()
self._online = online
# self.snd_handle = SoundClient()
if self._online:
#self._interface = ROSpeexInterface()
#self._interface.init()
self._speech_client = SpeechSynthesisClient_NICT()
else:
self.snd_handle = SoundClient()
rospy.sleep(1)
self.say('Hello world!')
rospy.sleep(1)
self._debug = debug
self._robot_frame = robot_frame
self._point_sub = rospy.Subscriber('nearest_face', PointStamped, self.face_cb)
self._head_action_cl = actionlib.SimpleActionClient('/head_traj_controller/point_head_action', pr2_controllers_msgs.msg.PointHeadAction)
self._torso_action_cl = actionlib.SimpleActionClient('/torso_controller/position_joint_action', pr2_controllers_msgs.msg.SingleJointPositionAction)
self._left_arm = MoveGroupCommander("left_arm")
self._right_arm = MoveGroupCommander("right_arm")
print "r.f.: " + self._left_arm.get_pose_reference_frame()
self.face = None
# self.face_from = rospy.Time(0)
self.face_last_dist = 0
self.last_invited_at = rospy.Time(0)
self._person_prob_left = 0
self.l_home_pose = [0.283, 0.295, 0.537, -1.646, 0.468, -1.735]
self.l_wave_1 = [-0.1, 0.6, 1.15, -1.7, -0.97, -1.6]
self.l_wave_2 = [-0.1, 0.6, 1.15, 1.7, -0.97, 1.6]
self.r_home_pose = [0.124, -0.481, 0.439, -1.548, 0.36, -0.035]
self.r_advert = [0.521, -0.508, 0.845, -1.548, 0.36, -0.035]
self.no_face_random_delay = None
self._initialized = False
self._timer = rospy.Timer(rospy.Duration(1.0), self.timer)
self._move_buff = Queue.Queue()
self._head_buff = Queue.Queue()
self._move_thread = threading.Thread(target=self.movements)
self._move_thread.daemon = True
self._move_thread.start()
self._head_thread = threading.Thread(target=self.head)
self._head_thread.daemon = True
self._head_thread.start()
self.new_face = False
self.face_last_dist = 0.0
self.face_counter = 0
self.actions = [self.stretchingAction,
self.introduceAction,
self.waveHandAction,
self.lookAroundAction,
self.lookAroundAction,
self.lookAroundAction,
self.advertAction,
self.numberOfFacesAction]
self.goodbye_strings = ["Thanks for stopping by.",
"Enjoy the event.",
"See you later!",
"Have a nice day!"]
self.invite_strings = ["Hello. It's nice to see you.",
"Come here and take some flyer.",
"I hope you are enjoying the event."]
rospy.loginfo("Ready")
def say(self, text):
if self._online:
#self._interface.say(text, 'en', 'nict')
data = self._speech_client.request(text, 'en')
try:
tmp_file = open('output_tmp.dat', 'wb')
tmp_file.write(data)
tmp_file.close()
# play sound
subprocess.check_output(['ffplay', 'output_tmp.dat', '-autoexit', '-nodisp'], stderr=subprocess.STDOUT)
except IOError:
rospy.logerr('file io error.')
except OSError:
rospy.logerr('ffplay is not installed.')
except subprocess.CalledProcessError:
rospy.logerr('ffplay return error value.')
except:
rospy.logerr('unknown exception.')
else:
self.snd_handle.say(text)
def getRandomFromArray(self, arr):
idx = random.randint(0, len(arr) - 1)
return arr[idx]
def stretchingAction(self):
self.say("I'm bit tired. Let's do some stretching.")
self._torso_action_cl.wait_for_server()
goal = pr2_controllers_msgs.msg.SingleJointPositionGoal()
goal.position = 0.195
goal.max_velocity = 0.5
try:
self._torso_action_cl.send_goal(goal)
self._torso_action_cl.wait_for_result(rospy.Duration.from_sec(10.0))
except:
rospy.logerr("torso action error (up)")
# TODO move arms
self.say("I feel much better now!")
goal.position = 0.0
try:
self._torso_action_cl.send_goal(goal)
self._torso_action_cl.wait_for_result(rospy.Duration.from_sec(10.0))
except:
rospy.logerr("torso action error (down)")
def numberOfFacesAction(self):
string = "Today I already saw " + str(self.face_counter) + "face"
if self.face_counter != 1:
string = string + "s"
string = string + "."
self.say(string)
rospy.sleep(1)
def advertAction(self):
self.say("Hello. Here are some posters for you.")
self.go(self._right_arm, self.r_advert)
rospy.sleep(1)
def introduceAction(self):
self.say("Hello. I'm PR2 robot. Come here to check me.")
rospy.sleep(1)
def waveHandAction(self):
self.say("I'm here. Please come to see me.")
rand = random.randint(1, 3)
for _ in range(rand):
self.wave()
self.go(self._left_arm, self.l_home_pose)
rospy.loginfo("Waving")
rospy.sleep(1)
def lookAroundAction(self):
self.say("I'm looking for somebody. Please come closer.")
p = PointStamped()
p.header.stamp = rospy.Time.now()
p.header.frame_id = "/base_link"
p.point.x = 5.0
sign = random.choice([-1, 1])
p.point.y = sign * random.uniform(1.5, 0.5)
p.point.z = random.uniform(1.7, 0.2)
self._head_buff.put((copy.deepcopy(p), 0.1, True))
p.point.y = -1 * sign * random.uniform(1.5, 0.5)
p.point.z = random.uniform(1.7, 0.2)
self._head_buff.put((copy.deepcopy(p), 0.1, True))
p.point.y = sign * random.uniform(1.5, 0.5)
p.point.z = random.uniform(1.7, 0.2)
self._head_buff.put((copy.deepcopy(p), 0.1, True))
p.point.y = -1 * sign * random.uniform(1.5, 0.5)
p.point.z = random.uniform(1.7, 0.2)
self._head_buff.put((copy.deepcopy(p), 0.1, True))
rospy.loginfo("Looking around")
rospy.sleep(1)
def getPointDist(self, pt):
assert(self.face is not None)
# fist, get my position
p = PoseStamped()
p.header.frame_id = "base_link"
p.header.stamp = rospy.Time.now() - rospy.Duration(0.5)
p.pose.position.x = 0
p.pose.position.y = 0
p.pose.position.z = 0
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
try:
self._tf.waitForTransform(p.header.frame_id, self._robot_frame, p.header.stamp, rospy.Duration(2))
p = self._tf.transformPose(self._robot_frame, p)
except:
rospy.logerr("TF error!")
return None
return sqrt(pow(p.pose.position.x - pt.point.x, 2) + pow(p.pose.position.y - pt.point.y, 2) + pow(p.pose.position.z - pt.point.z, 2))
def getPoseStamped(self, group, c):
assert(len(c) == 6)
p = PoseStamped()
p.header.frame_id = "base_link"
p.header.stamp = rospy.Time.now() - rospy.Duration(0.5)
p.pose.position.x = c[0]
p.pose.position.y = c[1]
p.pose.position.z = c[2]
quat = tf.transformations.quaternion_from_euler(c[3], c[4], c[5])
p.pose.orientation.x = quat[0]
p.pose.orientation.y = quat[1]
p.pose.orientation.z = quat[2]
p.pose.orientation.w = quat[3]
try:
self._tf.waitForTransform(p.header.frame_id, group.get_pose_reference_frame(), p.header.stamp, rospy.Duration(2))
p = self._tf.transformPose(group.get_pose_reference_frame(), p)
except:
rospy.logerr("TF error!")
return None
return p
def go(self, group, where):
self._move_buff.put((group, where))
def wave(self):
self.go(self._left_arm, self.l_wave_1)
self.go(self._left_arm, self.l_wave_2)
self.go(self._left_arm, self.l_wave_1)
def head(self):
self._head_action_cl.wait_for_server()
while not rospy.is_shutdown():
(target, vel, imp) = self._head_buff.get()
# we don't need to block it here
self._head_buff.task_done()
if (not imp) and (not self._head_buff.empty()):
print "skipping head goal"
# head target can be skipped (follow only the latest one)
continue
# print "head point goal"
# print target
# point PR2's head there (http://wiki.ros.org/pr2_controllers/Tutorials/Moving%20the%20Head)
goal = pr2_controllers_msgs.msg.PointHeadGoal()
goal.target = target
# goal.min_duration = rospy.Duration(3.0)
goal.max_velocity = vel
# goal.pointing_frame = "high_def_frame"
goal.pointing_frame = "head_mount_kinect_rgb_link"
goal.pointing_axis.x = 1
goal.pointing_axis.y = 0
goal.pointing_axis.z = 0
try:
self._head_action_cl.send_goal(goal)
self._head_action_cl.wait_for_result(rospy.Duration.from_sec(5.0))
except:
rospy.logerr("head action error")
#self._head_buff.task_done()
def movements(self):
while not rospy.is_shutdown():
(group, where) = self._move_buff.get()
group.set_start_state_to_current_state()
p = self.getPoseStamped(group, where)
if p is None:
self._move_buff.task_done()
continue
group.set_pose_target(p)
group.set_goal_tolerance(0.1)
group.allow_replanning(True)
self._move_buff.task_done()
group.go(wait=True)
def timer(self, event):
if self._initialized is False:
rospy.loginfo("Moving arms to home positions")
self.init_head()
self.go(self._left_arm, self.l_home_pose)
self.go(self._right_arm, self.r_home_pose)
self._move_buff.join()
self.say("I'm ready for a great job.")
self._initialized = True
if self.face is None:
if (self.no_face_random_delay is None):
delay = random.uniform(30, 10)
self.no_face_random_delay = rospy.Time.now() + rospy.Duration(delay)
rospy.loginfo("Random delay: " + str(delay))
return
else:
if rospy.Time.now() < self.no_face_random_delay:
return
self.init_head()
self.go(self._left_arm, self.l_home_pose)
self.go(self._right_arm, self.r_home_pose)
rospy.loginfo("Starting selected action")
action = self.getRandomFromArray(self.actions)
action()
delay = random.uniform(30, 10)
self.no_face_random_delay = rospy.Time.now() + rospy.Duration(delay)
return
else:
self.no_face_random_delay = None
if self.new_face and (self.last_invited_at + rospy.Duration(10) < rospy.Time.now()):
self.last_invited_at = rospy.Time.now()
self.new_face = False
rospy.loginfo("new person")
self.face_counter = self.face_counter + 1
# cd = getPointDist(self.face)
# TODO decide action based on distance ?
self.go(self._left_arm, self.l_home_pose) # if a person is too close, dont move hand?
self.go(self._right_arm, self.r_advert)
string = self.getRandomFromArray(self.invite_strings)
self.say(string)
# TODO wait some min. time + say something
# after 20 seconds of no detected face, let's continue
if self.face.header.stamp + rospy.Duration(10) < rospy.Time.now():
rospy.loginfo("person left")
string = self.getRandomFromArray(self.goodbye_strings)
self.say(string)
self.init_head()
self.go(self._left_arm, self.l_home_pose)
self.go(self._right_arm, self.r_home_pose)
self.face = None
return
self._head_buff.put((copy.deepcopy(self.face), 0.4, False))
def init_head(self):
p = PointStamped()
p.header.stamp = rospy.Time.now()
p.header.frame_id = "/base_link"
p.point.x = 2.0
p.point.y = 0.0
p.point.z = 1.7
self._head_buff.put((p, 0.1, True))
def face_cb(self, point):
# transform point
try:
self._tf.waitForTransform(point.header.frame_id, self._robot_frame, point.header.stamp, rospy.Duration(2))
pt = self._tf.transformPoint(self._robot_frame, point)
except:
rospy.logerr("Transform error")
return
if self.face is not None:
cd = self.getPointDist(pt) # current distance
dd = fabs(self.face_last_dist - cd) # change in distance
if dd < 0.5:
self.face.header = pt.header
# filter x,y,z values a bit
self.face.point = pt.point
#self.face.point.x = (self.face.point.x + pt.point.x) / 2
#self.face.point.y = (self.face.point.y + pt.point.y) / 2
#self.face.point.z = (self.face.point.z + pt.point.z) / 2
else:
if self._person_prob_left < 2:
self._person_prob_left += 1
else:
self._person_prob_left = 0
print "new face ddist: " + str(dd)
self.new_face = True
self.face = pt
self.face_last_dist = cd
else:
self._person_prob_left = 0
self.new_face = True
self.face = pt
