def update():
"""Checks whether the navigation is finished.
"""
replan_expired = (
self.replan_time is not None
and (rospy.Time.now() - self.replan_time).to_sec() > 4.0)
curr_pose = get_pose_stamped(self.tf_buffer)
near_goal = (curr_pose is not None
and (Place(pose_stamped=curr_pose).to(
Place(pose_stamped=goal_pose)) < 0.1))
self.is_finished = (replan_expired or near_goal)
def closest(self, m):
"""Return the closest place and distance to q.
Args:
m: WorldClosest message.
"""
place, distance = self.helper.closest(Place(msg=m.origin))
return WorldClosestResponse(place.msg, distance)
def _load(self):
"""Load the JSON database into memory.
"""
if os.path.isfile(self.fname):
with open(self.fname, "r") as f:
self.content = [Place(json=x) for x in json.load(f)]
def append(self, m):
"""Add a new place to memory.
Args:
m: WorldAppend message.
"""
self.helper.append(Place(msg=m.origin))
return WorldAppendResponse()
def closest(self, q):
"""Return the closest place and distance to q.
Args:
q: Place object.
"""
result = self.closest_service(q.msg)
return Place(msg=result.match), result.distance
def lookup(self, x):
"""If this place exists, return it.
Args:
x: str
"""
result = self.lookup_service(x)
return (Place(msg=result.match) if result.found else None)
def lookup(self, m):
"""If this place exists, return it.
Args:
m: WorldLookup message.
"""
place = self.helper.lookup(m.name)
if place is not None:
return WorldLookupResponse(place.msg, True)
return WorldLookupResponse(Place(name="None", x=-1, y=-1).msg, False)
def where(self):
"""Check the robots map find the closest place.
Example:
Where place are you.
"""
curr_pose = get_pose_stamped(self.tf_buffer)
if not curr_pose:
return LOST_STRING
p, distance = self.places.closest(Place(pose_stamped=curr_pose))
return WHERE_STRING.format(p.name, distance)
def closest(self, q):
"""Return the closest place and distance to q.
"""
best_place = Place(name="None", x=-1, y=-1)
best_distance = float("inf")
for p in self.content:
current_distance = q.to(p)
if current_distance < best_distance:
best_place = p
best_distance = current_distance
return best_place, best_distance
def learn(self, name):
"""Learn the robots position as the given name.
Example:
This location is my "Office".
"""
destination = self.places.lookup(name)
if destination:
return KNOWN_STRING.format(name)
curr_pose = get_pose_stamped(self.tf_buffer)
if not curr_pose:
return LOST_STRING
self.places.append(Place(name=name, pose_stamped=curr_pose))
return LEARN_STRING.format(name)
def navigate(self, name):
"""The user asked to navigate to a location.
Example:
Navigate to the "Office".
"""
destination = self.places.lookup(name)
if not destination:
return UNK_STRING.format(name, self.places.length(),
self.places.string())
curr_pose = get_pose_stamped(self.tf_buffer)
if not curr_pose:
return LOST_STRING
self.navigator.go(destination.pose_stamped)
return NAV_STRING.format(name,
Place(pose_stamped=curr_pose).to(destination))