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))