def execute_callback(self, goal):
rospy.loginfo('%s rendering requested [%s]...' %
(rospy.get_name(), goal.data))
result = RenderItemResult()
feedback = RenderItemFeedback()
result.result = True
if not self.u_initialized:
rospy.sleep(0.2)
result.result = True
self.server.set_succeeded(result)
return
self.is_speaking = True
self.pub_status.publish(True)
feedback.is_rendering = True
internal_goal = SpeechGoal()
internal_goal.text = goal.data
self.internal_client.send_goal(internal_goal,
done_cb=self.func_done,
feedback_cb=self.func_feedback)
while not rospy.is_shutdown() and self.is_speaking:
if self.server.is_preempt_requested():
self.server.set_preempted()
result.result = False
break
self.server.publish_feedback(feedback)
rospy.sleep(0.2)
if result.result:
self.server.set_succeeded(result)
def execute_callback(self, goal):
result = RenderItemResult()
feedback = RenderItemFeedback()
success = True
self.last_eye_color = self.current_eye_color
try:
self.current_eye_color = self.eye_color[goal.data]
self.proxy.fadeRGB("FaceLeds", self.current_eye_color, 0.02)
except KeyError:
rospy.logwarn('render %s expression is not defined...' % goal.data)
pass
rospy.sleep(0.2)
if success:
result.result = True
self.server.set_succeeded(result)
rospy.loginfo('\033[95m%s\033[0m rendering completed...' %
rospy.get_name())
def execute_callback(self, goal):
result = RenderItemResult()
feedback = RenderItemFeedback()
success = True
rospy.sleep(0.5)
led_msg = Device_LED_Msg()
led_msg.command = "on"
led_msg.id = 0xF5
led_msg.bright = 1
led_msg.red = 128 #self.led_color[goal.data][0]
led_msg.green = self.led_color[goal.data][1]
led_msg.blue = self.led_color[goal.data][2]
self.led_publisher.publish(led_msg)
rospy.sleep(1)
# Facial Expression
if success:
result.result = True
self.server.set_succeeded(result)
def render_gesture(self, goal):
rospy.logdebug("Execution of behavior: '{}' requested".format(
goal.data))
result = RenderItemResult()
feedback = RenderItemFeedback()
succeed = True
gesture_type, gesture_data = goal.data.split('=')
if gesture_type == 'gesture':
(cmd, item_name) = gesture_data.split(':')
if cmd == 'tag':
match = re.search(r'\[(.+?)\]', item_name)
rendering_gesture = ''
if match:
item_name = item_name.replace(match.group(0), '')
emotion = match.group(1)
try:
rendering_gesture = self.motion_list[item_name][
emotion][random.randrange(
0,
len(self.motion_list[item_name]) - 1)]
except (KeyError, TypeError):
rendering_gesture = self.motion_list[item_name][
random.randint(
0,
len(self.motion_list[item_name]) - 1)]
else:
try:
rendering_gesture = self.motion_list[item_name][
random.randint(
0,
len(self.motion_list[item_name]) - 1)]
except KeyError:
rendering_gesture = self.motion_list['neutral'][
random.randint(
0,
len(self.motion_list[item_name]) - 1)]
with self.lock:
if self.actionlibServer.is_preempt_requested():
self.actionlibServer.set_preempted()
rospy.logdebug(
"Gesture execution preempted before it started")
return
self.gesture = rendering_gesture
taskID = self.behaviorProxy.post.runBehavior(self.gesture)
rospy.logdebug("Waiting for behavior execution to complete")
while self.behaviorProxy.isRunning(
taskID) and not rospy.is_shutdown():
rospy.sleep(0.2)
with self.lock:
self.gesture = None
succeed = True
elif cmd == 'play':
found_motion = False
for k, v in self.motion_list.items():
if item_name in v:
found_motion = True
if not found_motion:
error_msg = "Gesture '{}' not installed".format(item_name)
self.actionlibServer.set_aborted(text=error_msg)
rospy.logerr(error_msg)
return
else:
self.gesture = item_name
taskID = self.behaviorProxy.post.runBehavior(self.gesture)
rospy.logdebug(
"Waiting for behavior execution to complete")
while self.behaviorProxy.isRunning(
taskID) and not rospy.is_shutdown():
rospy.sleep(0.2)
with self.lock:
self.gesture = None
succeed = True
elif gesture_type == 'pointing':
parse_data = json.loads(gesture_data)
rospy.loginfo(
'\033[94m[%s]\033[0m rendering pointing to xyz:%s, frame_id [%s]...'
%
(rospy.get_name(), parse_data['xyz'], parse_data['frame_id']))
target = PointStamped()
target.header.frame_id = parse_data['frame_id']
target.point.x = parse_data['xyz'][0]
target.point.y = parse_data['xyz'][1]
target.point.z = parse_data['xyz'][2]
try:
point_transformed = self.tf_buf.transform(target, 'torso')
self.motionProxy.pointAt("RArm", [
point_transformed.point.x, point_transformed.point.y,
point_transformed.point.z
], 0, 0.3)
rospy.sleep(1)
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
rospy.logwarn(
'The transform information can not find with /RShoulder and target point...'
)
succeed = False
else:
rospy.sleep(1.0)
succeed = False
if succeed:
result.result = True
self.actionlibServer.set_succeeded(result)
rospy.loginfo('\033[95m%s\033[0m rendering completed...' %
rospy.get_name())
def execute_callback(self, goal):
rospy.loginfo('%s rendering requested [%s]...' %
(rospy.get_name(), goal.data))
result = RenderItemResult()
feedback = RenderItemFeedback()
result.result = True
msg = TTSSetProperties()
msg.speaker_id = 0 #SPEAKER_ID_ENGLISH_FEMALE=3, SPEAKER_ID_ENGLISH_MALE=4
msg.speed = 100
msg.volume = 100
msg.pitch = 100
self.set_publisher.publish(msg)
filename = '/tmp/speech.wav'
req = TTSMakeRequest()
req.text = goal.data
req.filepath = filename
res = self.make_service_client(req)
rospy.logdebug("make_tts : " + str(res))
if res.tts_result != 0:
return
feedback.is_rendering = True
self.server.publish_feedback(feedback)
with noalsaerr():
paudio = pyaudio.PyAudio()
wave_file = wave.open(filename, 'rb')
try:
stream = paudio.open(format=paudio.get_format_from_width(
wave_file.getsampwidth()),
channels=wave_file.getnchannels(),
rate=wave_file.getframerate(),
output=True)
except IOError:
return
chunk_size = 1024
data = wave_file.readframes(chunk_size)
while data != '':
if self.server.is_preempt_requested():
self.server.set_preempted()
result.result = False
break
stream.write(data)
data = wave_file.readframes(chunk_size)
self.server.publish_feedback(feedback)
stream.stop_stream()
stream.close()
paudio.terminate()
rospy.sleep(0.2)
if result.result:
self.server.set_succeeded(result)
def execute_callback(self, goal):
rospy.loginfo('\033[95m%s\033[0m rendering requested...' % rospy.get_name())
result = RenderItemResult()
feedback = RenderItemFeedback()
success = True
loop_count = 0
if 'render_gesture' in rospy.get_name():
(gesture_category, gesture_item) = goal.data.split('=')
if gesture_category == 'pointing':
parse_data = json.loads(gesture_item)
rospy.loginfo('\033[94m[%s]\033[0m rendering pointing to xyz:%s, frame_id [%s]...'%(rospy.get_name(),
parse_data['xyz'], parse_data['frame_id']))
elif gesture_category == 'gesture':
(cmd, item_name) = gesture_item.split(':')
if cmd == 'tag':
match = re.search(r'\[(.+?)\]', item_name)
if match:
item_name = item_name.replace(match.group(0), '')
emotion = match.group(1)
try:
rospy.loginfo('\033[94m[%s]\033[0m rendering gesture cmd [%s], name [%s]...'%(rospy.get_name(),
cmd,
self.motion_list[item_name][emotion][random.randint(0, len(self.motion_list[item_name]) - 1)]))
except (KeyError, TypeError):
rospy.loginfo('\033[94m[%s]\033[0m rendering gesture cmd [%s], name [%s]...'%(rospy.get_name(),
cmd,
self.motion_list[item_name][random.randint(0, len(self.motion_list[item_name]) - 1)]))
else:
try:
rospy.loginfo('\033[94m[%s]\033[0m rendering gesture cmd [%s], name [%s]...'%(rospy.get_name(),
cmd,
self.motion_list[item_name][random.randint(0, len(self.motion_list[item_name]) - 1)]))
except KeyError:
rospy.logwarn('\033[94m[%s]\033[0m rendering gesture cmd [%s], name [%s]...'%(rospy.get_name(),
cmd,
self.motion_list['neutral'][random.randint(0, len(self.motion_list['neutral']) - 1)]))
elif cmd == 'play':
find_result = False
for k, v in self.motion_list.items():
if item_name in v:
find_result = True
if find_result:
rospy.loginfo('\033[94m[%s]\033[0m rendering gesture cmd [%s], name [%s]...'%(rospy.get_name(),
cmd, item_name))
else:
rospy.logwarn('\033[94m[%s]\033[0m rendering gesture cmd [%s], name [%s]...'%(rospy.get_name(),
cmd,
self.motion_list['neutral'][random.randint(0, len(self.motion_list['neutral']) - 1)]))
loop_count = 10
if 'render_speech' in rospy.get_name():
rospy.loginfo('\033[94m[%s]\033[0m rendering speech [%s]...'%(rospy.get_name(), goal.data))
loop_count = 10
if 'render_screen' in rospy.get_name():
rospy.loginfo('\033[94m[%s]\033[0m rendering screen [%s]...'%(rospy.get_name(), goal.data))
loop_count = 10
if 'render_mobility' in rospy.get_name():
rospy.loginfo('\033[94m[%s]\033[0m rendering mobility [%s]...'%(rospy.get_name(), goal.data))
loop_count = 10
if 'render_facial_expression' in rospy.get_name():
rospy.loginfo('\033[94m[%s]\033[0m rendering expression [%s]...'%(rospy.get_name(), goal.data))
loop_count = 5
while not rospy.is_shutdown():
if self.server.is_preempt_requested():
self.server.set_preempted()
success = False
break
feedback.is_rendering = True
self.server.publish_feedback(feedback)
rospy.sleep(0.1)
loop_count = loop_count - 1
if loop_count == 0:
break
if success:
result.result = True
self.server.set_succeeded(result)
rospy.loginfo('\033[95m%s\033[0m rendering completed...' % rospy.get_name())
else:
rospy.loginfo('\033[95m%s\033[0m rendering canceled...' % rospy.get_name())
def execute_callback(self, goal):
result = RenderItemResult()
feedback = RenderItemFeedback()
success = True
if ':' in goal.data:
(move_cmd, target_waypoint) = goal.data.split(':')
else:
move_cmd = goal.data
if move_cmd == 'move':
rospy.loginfo('start moving base to %s...' % target_waypoint)
move_goal = MoveBaseGoal()
move_goal.target_pose.header.stamp = rospy.Time.now()
move_goal.target_pose.header.frame_id = 'map'
move_goal.target_pose.pose.position.x = self.waypoints[
target_waypoint][0]
move_goal.target_pose.pose.position.y = self.waypoints[
target_waypoint][1]
move_goal.target_pose.pose.position.z = 0.0
quat = tf.transformations.quaternion_from_euler(
0.0, 0.0, self.waypoints[target_waypoint][2])
move_goal.target_pose.pose.orientation.x = quat[0]
move_goal.target_pose.pose.orientation.y = quat[1]
move_goal.target_pose.pose.orientation.z = quat[2]
move_goal.target_pose.pose.orientation.w = quat[3]
rospy.wait_for_service('/move_base/clear_costmaps')
clear_map = rospy.ServiceProxy('/move_base/clear_costmaps', Empty)
clear_map()
self.client.send_goal(move_goal,
done_cb=self.handle_moving_done,
feedback_cb=self.handle_moving_feedback)
self.is_mobile_moving = True
while self.is_mobile_moving:
if self.server.is_preempt_requested():
self.client.cancel_all_goals()
self.server.set_preempted()
rospy.loginfo('stop moving by request...')
return
feedback.is_rendering = True
self.server.publish_feedback(feedback)
rospy.sleep(0.2)
rospy.sleep(0.2)
rospy.loginfo('complete moving base...')
elif move_cmd is 'stop':
rospy.loginfo('stop moving...')
self.client.cancel_all_goals()
if success:
result.result = True
self.server.set_succeeded(result)
def execute_callback(self, goal):
result = RenderItemResult()
feedback = RenderItemFeedback()
success = True
self.pub_enable_gaze.publish(False)
gesture_type, gesture_data = goal.data.split('=')
if gesture_type == 'gesture':
(cmd, item_name) = gesture_data.split(':')
if cmd == 'tag':
match = re.search(r'\[(.+?)\]', item_name)
rendering_gesture = ''
if match:
item_name = item_name.replace(match.group(0), '')
emotion = match.group(1)
try:
rendering_gesture = self.motion_list[item_name][emotion][random.randrange(0, len(self.motion_list[item_name]) - 1)]
except (KeyError, TypeError):
rendering_gesture = self.motion_list[item_name][random.randint(0, len(self.motion_list[item_name]) - 1)]
else:
try:
rendering_gesture = self.motion_list[item_name][random.randint(0, len(self.motion_list[item_name]) - 1)]
except KeyError:
rendering_gesture = self.motion_list['neutral'][random.randint(0, len(self.motion_list[item_name]) - 1)]
with self.lock:
if self.server.is_preempt_requested():
self.server.set_preempted()
rospy.logdebug("Gesture execution preempted before it started")
return
self.gesture = rendering_gesture
split_data = self.gesture.split('/')
req = ExpressionStartRequest()
req.expression_type = 0
req.package = split_data[0]
req.category = split_data[1]
req.id = split_data[2]
req.content = ''
res = self.startMotion(req)
self.motionFinished = False
rospy.sleep(0.1)
rospy.loginfo("Waiting for behavior execution to complete")
while not self.motionFinished and not rospy.is_shutdown():
rospy.sleep(0.2)
with self.lock:
self.gesture = None
succeed = True
elif cmd == 'play':
found_motion = False
for k, v in self.motion_list.items():
if item_name in v:
found_motion = True
if not found_motion:
error_msg = "Gesture '{}' not installed".format(item_name)
self.server.set_aborted(text = error_msg)
rospy.logerr(error_msg)
return
else:
self.gesture = item_name
split_data = self.gesture.split('/')
req = ExpressionStartRequest()
req.expression_type = 0
req.package = split_data[0]
req.category = split_data[1]
req.id = split_data[2]
req.content = ''
res = self.startMotion(req)
self.motionFinished = False
rospy.sleep(0.1)
rospy.loginfo("Waiting for behavior execution to complete")
while not self.motionFinished and not rospy.is_shutdown():
rospy.sleep(0.2)
with self.lock:
self.gesture = None
succeed = True
if success:
result.result = True
self.motionFinished = False
self.pub_enable_gaze.publish(True)
self.server.set_succeeded(result)