Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

separate operator case so we do not need to resolve entity #159

Merged
merged 4 commits into from
Jan 3, 2022
Merged

separate operator case so we do not need to resolve entity #159

Changes from 2 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
f198d96
separate operator case so we do not need to resolve entity
Dec 7, 2021
15f529d
remove #todo
PetervDooren Dec 14, 2021
a837327
remove unneeded check
PetervDooren Dec 14, 2021
8abbade
check if entity is not none
PetervDooren Dec 14, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 17 additions & 9 deletions action_server/src/action_server/actions/navigate_to.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -51,12 +51,6 @@ def _parse_context(context_dict):
def _configure(self, robot, config):
self._robot = robot

# If we need to navigate to "me", which resolves to "operator", plant a waypoint at the current position to
# navigate to.
# TODO: learn to recognize the operator so that you know you found him later on
self._robot.ed.update_entity(uuid="operator", frame_stamped=self._robot.base.get_location(),
etype="waypoint")

semantics = self._parse_semantics(config.semantics)
context = self._parse_context(config.context)

Expand Down Expand Up @@ -94,11 +88,23 @@ def _configure(self, robot, config):
self._config_result.required_context['source-location'] = config.semantics['source-location']
return
# Now we can assume we know the navigation goal entity!
# operator
if semantics.target_location.id and semantics.target_location.id == 'operator':
# If we need to navigate to "me", which resolves to "operator", plant a waypoint at the current position to
# navigate to.
# TODO: learn to recognize the operator so that you know you found him later on
self._robot.ed.update_entity(uuid="operator", frame_stamped=self._robot.base.get_location(),
PetervDooren marked this conversation as resolved.
Show resolved Hide resolved
etype="waypoint")
entity_designator = EntityByIdDesignator(self._robot, uuid=semantics.target_location.id)
self._navigation_state_machine = NavigateToWaypoint(self._robot,
waypoint_designator=entity_designator,
radius=0.1)
rospy.loginfo("Navigation set up for a waypoint")

if semantics.target_location.id and \
(semantics.target_location.type != 'person' or semantics.target_location.id == 'operator'):
# known room or object
elif semantics.target_location.id and semantics.target_location.type != 'person':
entity_designator = EntityByIdDesignator(self._robot, uuid=semantics.target_location.id)
e = entity_designator.resolve() # TODO: nasty assumption that we can resolve this entity here?!
e = entity_designator.resolve()

if e.is_a("waypoint"):
self._navigation_state_machine = NavigateToWaypoint(self._robot,
Expand All @@ -119,12 +125,14 @@ def _configure(self, robot, config):
entity_designator_area_name_map={
entity_designator: area},
entity_lookat_designator=entity_designator)
# person from context
elif semantics.target_location.type == 'person' and context.object:
entity_designator = context.object.designator
self._navigation_state_machine = NavigateToWaypoint(self._robot,
waypoint_designator=entity_designator,
radius=0.7)
rospy.loginfo("Navigation set up for a waypoint")
# entity from context
else:
entity_designator = context.object.designator
self._navigation_state_machine = NavigateToSymbolic(self._robot,
Expand Down