diff --git a/cabot_ui/cabot_ui/datautil.py b/cabot_ui/cabot_ui/datautil.py index 1bb707f9..8229f50a 100644 --- a/cabot_ui/cabot_ui/datautil.py +++ b/cabot_ui/cabot_ui/datautil.py @@ -206,9 +206,10 @@ def analyze_features(self): # links = geojson.Object.get_objects_by_type(geojson.Link) doors = geojson.Object.get_objects_by_type(geojson.DoorPOI) infos = geojson.Object.get_objects_by_type(geojson.InfoPOI) + entrances = geojson.Object.get_objects_by_type(geojson.EntrancePOI) speeds = geojson.Object.get_objects_by_type(geojson.SpeedPOI) - for poi in doors+infos+speeds: + for poi in doors+infos+entrances+speeds: min_link, min_dist = geojson.Object.get_nearest_link(poi) if min_link is None: CaBotRclpyUtil.debug(F"poi {poi._id} ({poi.floor}) is not registered.") diff --git a/cabot_ui/cabot_ui/geojson.py b/cabot_ui/cabot_ui/geojson.py index 054fc697..f39ad721 100644 --- a/cabot_ui/cabot_ui/geojson.py +++ b/cabot_ui/cabot_ui/geojson.py @@ -41,6 +41,7 @@ import geometry_msgs.msg from cabot_ui import geoutil, i18n from cabot_ui.cabot_rclpy_util import CaBotRclpyUtil +from cabot_ui.interface import SpeechPriority class RouteStruct(enum.Enum): @@ -628,6 +629,10 @@ def __init__(self, facility, i, node): def floor(self): return self.node.floor + @property + def speech_priority(self): + return SpeechPriority.LOW + def update_anchor(self, anchor): self.anchor = anchor @@ -761,6 +766,8 @@ def marshal(cls, dic): cls = DoorPOI if category == '_nav_info_': cls = InfoPOI + if category == '_nav_entrance_': + cls = EntrancePOI if category == '_cabot_speed_': cls = SpeedPOI if category == '_nav_elevator_cab_': @@ -797,6 +804,10 @@ def _get_prop(self, key): def floor(self): return self._get_prop('hulop_height') + @property + def speech_priority(self): + return SpeechPriority.REQUIRED + def approaching_statement(self): return None @@ -875,6 +886,25 @@ def approached_statement(self): return self.name +class EntrancePOI(POI): + """Nav Entrance POI class""" + + @classmethod + def marshal(cls, dic): + """marshal Entrance POI object""" + return cls(**dic) + + def __init__(self, **dic): + super(EntrancePOI, self).__init__(**dic) + + def approached_statement(self): + return self.name + + @property + def speech_priority(self): + return SpeechPriority.LOW + + class SpeedPOI(POI): """Cabot Speed POI class""" diff --git a/cabot_ui/cabot_ui/interface.py b/cabot_ui/cabot_ui/interface.py index f01fa8c4..c61ffe8b 100644 --- a/cabot_ui/cabot_ui/interface.py +++ b/cabot_ui/cabot_ui/interface.py @@ -37,9 +37,20 @@ from cabot_common import vibration +class SpeechPriority: + REQUIRED = 90 + HIGH = 60 + NORMAL = 30 + LOW = 10 + + class UserInterface(object): SOCIAL_ANNOUNCE_INTERVAL = Duration(seconds=15.0) NOTIFY_TURN_INTERVAL = Duration(seconds=5.0) + PRIORITY_REQUIRED = 90 + PRIORITY_HIGH = 60 + PRIORITY_NORMAL = 30 + PRIORITY_LOW = 10 def __init__(self, node: Node): self._node = node @@ -106,7 +117,7 @@ def change_language(self, lang): self.lang = lang i18n.set_language(self.lang) - def speak(self, text, force=True, pitch=50, volume=50, rate=50): + def speak(self, text, force=True, pitch=50, volume=50, rate=50, priority=SpeechPriority.NORMAL): if text is None: return @@ -121,7 +132,7 @@ def speak(self, text, force=True, pitch=50, volume=50, rate=50): if not speak_proxy.wait_for_service(timeout_sec=1): CaBotRclpyUtil.error("Service cannot be found") return - CaBotRclpyUtil.info(F"try to speak {text} (v={voice}, r={rate}, p={pitch}) {force}") + CaBotRclpyUtil.info(F"try to speak {text} (v={voice}, r={rate}, p={pitch}, priority={priority}) {force}") request = cabot_msgs.srv.Speak.Request() request.text = text request.rate = rate @@ -130,7 +141,7 @@ def speak(self, text, force=True, pitch=50, volume=50, rate=50): request.lang = self.lang request.voice = voice request.force = force - request.priority = 50 + request.priority = priority request.timeout = 2.0 request.channels = cabot_msgs.srv.Speak.Request.CHANNEL_BOTH speak_proxy.call_async(request) @@ -179,21 +190,21 @@ def menu_changed(self, menu=None, backed=False, usage=False): def pause_navigation(self): self._activity_log("cabot/interface", "navigation", "pause") - self.speak(i18n.localized_string("PAUSE_NAVIGATION")) + self.speak(i18n.localized_string("PAUSE_NAVIGATION"), priority=SpeechPriority.REQUIRED) def cancel_navigation(self): - pass # self.speak(i18n.localized_string("CANCEL_NAVIGATION")) + pass # self.speak(i18n.localized_string("CANCEL_NAVIGATION"), priority=SpeechPriority.REQUIRED) def resume_navigation(self): self._activity_log("cabot/interface", "navigation", "resume") - self.speak(i18n.localized_string("RESUME_NAVIGATION")) + self.speak(i18n.localized_string("RESUME_NAVIGATION"), priority=SpeechPriority.REQUIRED) def pausing_navigation(self): self._activity_log("cabot/interface", "navigation", "pausing") - self.speak(i18n.localized_string("PLEASE_WAIT_FOR_A_SECOND")) + self.speak(i18n.localized_string("PLEASE_WAIT_FOR_A_SECOND"), priority=SpeechPriority.REQUIRED) def start_exploration(self): - pass # self.speak(i18n.localized_string("START_EXPLORATION")) + pass # self.speak(i18n.localized_string("START_EXPLORATION"), priority=SpeechPriority.REQUIRED) # navigate interface @@ -202,11 +213,11 @@ def activity_log(self, category="", text="", memo=""): def in_preparation(self): self._activity_log("cabot/interface", "status", "prepare") - self.speak(i18n.localized_string("IN_PRERARATION")) + self.speak(i18n.localized_string("IN_PRERARATION"), priority=SpeechPriority.REQUIRED) def i_am_ready(self): self._activity_log("cabot/interface", "status", "ready") - self.speak(i18n.localized_string("I_AM_READY")) + self.speak(i18n.localized_string("I_AM_READY"), priority=SpeechPriority.REQUIRED) def start_navigation(self): self._activity_log("cabot/interface", "navigation", "start") @@ -265,7 +276,7 @@ def notify_human(self, angle=0): self._activity_log("cabot/interface", "human") self.vibrate(pattern=vib) - self.speak(i18n.localized_string("AVOIDING_A_PERSON")) + self.speak(i18n.localized_string("AVOIDING_A_PERSON"), priority=SpeechPriority.NORMAL) def have_arrived(self, goal): raise RuntimeError("Should no use this func") @@ -274,33 +285,33 @@ def have_arrived(self, goal): if name: if desc: - self.speak(i18n.localized_string("YOU_HAVE_ARRIVED_WITH_NAME_AND_DESCRIPTION").format(name, desc)) + self.speak(i18n.localized_string("YOU_HAVE_ARRIVED_WITH_NAME_AND_DESCRIPTION").format(name, desc), priority=SpeechPriority.HIGH) else: - self.speak(i18n.localized_string("YOU_HAVE_ARRIVED_WITH_NAME").format(name)) + self.speak(i18n.localized_string("YOU_HAVE_ARRIVED_WITH_NAME").format(name), priority=SpeechPriority.HIGH) else: - self.speak(i18n.localized_string("YOU_HAVE_ARRIVED")) + self.speak(i18n.localized_string("YOU_HAVE_ARRIVED"), priority=SpeechPriority.HIGH) self._activity_log("cabot/interface", "navigation", "arrived") def approaching_to_poi(self, poi=None): statement = poi.approaching_statement() if statement: - self.speak(statement) + self.speak(statement, priority=poi.speech_priority) self._activity_log("cabot/interface", "poi", "approaching") def approached_to_poi(self, poi=None): statement = poi.approached_statement() if statement: - self.speak(statement) + self.speak(statement, priority=poi.speech_priority) self._activity_log("cabot/interface", "poi", "approached") def passed_poi(self, poi=None): statement = poi.passed_statement() if statement: - self.speak(statement) + self.speak(statement, priority=poi.speech_priority) self._activity_log("cabot/interface", "poi", "passed") def could_not_get_current_location(self): - self.speak(i18n.localized_string("COULD_NOT_GET_CURRENT_LOCATION")) + self.speak(i18n.localized_string("COULD_NOT_GET_CURRENT_LOCATION"), priority=SpeechPriority.REQUIRED) def enter_goal(self, goal): pass @@ -322,28 +333,28 @@ def request_sound(self, sound: SNMessage): def set_pause_control(self, flag): self._activity_log("cabot/interface", "pause_control", str(flag)) if flag: - self.speak(i18n.localized_string("PAUSE_CONTROL")) + self.speak(i18n.localized_string("PAUSE_CONTROL"), priority=SpeechPriority.REQUIRED) def please_call_elevator(self, pos): self._activity_log("cabot/interface", "navigation", "elevator button") if pos: self.speak(i18n.localized_string("CALL_ELEVATOR_PLEASE_ON_YOUR", - i18n.localized_string(pos))) + i18n.localized_string(pos)), priority=SpeechPriority.REQUIRED) else: - self.speak(i18n.localized_string("CALL_ELEVATOR_PLEASE")) + self.speak(i18n.localized_string("CALL_ELEVATOR_PLEASE"), priority=SpeechPriority.REQUIRED) def elevator_opening(self): self._activity_log("cabot/interface", "navigation", "elevator opening") self.vibrate(vibration.FRONT) - self.speak(i18n.localized_string("ELEVATOR_IS_OPENING")) + self.speak(i18n.localized_string("ELEVATOR_IS_OPENING"), priority=SpeechPriority.REQUIRED) def floor_changed(self, floor): self._activity_log("cabot/interface", "navigation", "floor_changed") - self.speak(i18n.localized_string("GETTING_OFF_THE_ELEVATOR")) + self.speak(i18n.localized_string("GETTING_OFF_THE_ELEVATOR"), priority=SpeechPriority.REQUIRED) def queue_start_arrived(self): self._activity_log("cabot/interface", "queue", "start arrived") - self.speak(i18n.localized_string("GOING_TO_GET_IN_LINE")) + self.speak(i18n.localized_string("GOING_TO_GET_IN_LINE"), priority=SpeechPriority.REQUIRED) def queue_proceed(self): self._activity_log("cabot/interface", "queue", "proceed") @@ -351,24 +362,24 @@ def queue_proceed(self): def please_pass_door(self): self._activity_log("cabot/interface", "navigation", "manual door") - self.speak(i18n.localized_string("DOOR_POI_USER_ACTION")) + self.speak(i18n.localized_string("DOOR_POI_USER_ACTION"), priority=SpeechPriority.REQUIRED) def door_passed(self): self._activity_log("cabot/interface", "navigation", "door passed") - self.speak(i18n.localized_string("DOOR_POI_PASSED")) + self.speak(i18n.localized_string("DOOR_POI_PASSED"), priority=SpeechPriority.REQUIRED) def please_follow_behind(self): self._activity_log("cabot/interface", "navigation", "please_follow_behind") - self.speak(i18n.localized_string("FOLLOW_BEHIND_PLEASE_NARROW")) + self.speak(i18n.localized_string("FOLLOW_BEHIND_PLEASE_NARROW"), priority=SpeechPriority.REQUIRED) def please_return_position(self): self._activity_log("cabot/interface", "navigation", "please_return_position") - self.speak(i18n.localized_string("RETURN_TO_POSITION_PLEASE")) + self.speak(i18n.localized_string("RETURN_TO_POSITION_PLEASE"), priority=SpeechPriority.REQUIRED) def requesting_describe_surround(self): self._activity_log("cabot/interface", "requesting_describe_surround", "") - self.speak(i18n.localized_string("REQUESTING_DESCRIBE_SURROUND")) + self.speak(i18n.localized_string("REQUESTING_DESCRIBE_SURROUND"), priority=SpeechPriority.HIGH) def describe_surround(self, description): self._activity_log("cabot/interface", "describe_surround", description) - self.speak(description) + self.speak(description, priority=SpeechPriority.HIGH) diff --git a/cabot_ui/cabot_ui/visualizer.py b/cabot_ui/cabot_ui/visualizer.py index 00b05c46..9d4cd44c 100644 --- a/cabot_ui/cabot_ui/visualizer.py +++ b/cabot_ui/cabot_ui/visualizer.py @@ -110,6 +110,10 @@ def make_marker2(ns=None, pose=None, array.markers.append(make_marker(poi, b=1.0, x=1.0, s=0.5, _type=Marker.ARROW)) array.markers.append(make_marker(poi, text=poi.approached_statement(), _type=Marker.TEXT_VIEW_FACING)) + elif isinstance(poi, geojson.EntrancePOI): + array.markers.append(make_marker(poi, b=1.0, x=1.0, s=0.5, _type=Marker.ARROW)) + array.markers.append(make_marker(poi, text=poi.approached_statement(), + _type=Marker.TEXT_VIEW_FACING)) elif isinstance(poi, geojson.SpeedPOI): array.markers.append(make_marker(poi, r=1.0, x=1.0, s=0.5, _type=Marker.ARROW)) array.markers.append(make_marker(poi, text="%.2f m/s" % (poi.limit),