def executeSpeechWithFeedbackAction(self, goal): self.speech_with_feedback_flag = True success = True result = RenderItemResult() saystr = goal.data if goal.data == '': rospy.sleep(0.5) result.result = True self.speechWithFeedbackServer.set_succeeded(result) return else: self.internalSay(saystr) counter = 0 while self.speech_with_feedback_flag == True and counter < 1200: if self.speechWithFeedbackServer.is_preempt_requested(): self.tts.stopAll() self.speechWithFeedbackServer.set_preempted() return rospy.sleep(0.1) counter += 1 if success: rospy.sleep(0.2) result.result = True self.speechWithFeedbackServer.set_succeeded(result)
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() success = True data = goal.data.split('/') if ':' in data[0]: screen_cmd, screen_name = data[0].split(':') screen_data = data[1] else: screen_cmd = data[0] screen_name = '' screen_data = '{}' if screen_cmd == 'clear': self.signalHandleCommand.emit(False) elif screen_cmd == 'show': self.signalHandleCommand.emit(True) self.signalHandleView.emit(screen_name, screen_data) rospy.sleep(0.2) if success: result.result = True self.server.set_succeeded(result)
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)