From edea4e8c565003769fca4d4572c253708cc63ae2 Mon Sep 17 00:00:00 2001 From: Daisuke Sato Date: Tue, 25 Jun 2024 17:23:57 -0400 Subject: [PATCH] ignore facility close to goal Signed-off-by: Daisuke Sato --- cabot_ui/cabot_ui/geojson.py | 1 + cabot_ui/cabot_ui/navigation.py | 22 ++++++++++++++-------- 2 files changed, 15 insertions(+), 8 deletions(-) diff --git a/cabot_ui/cabot_ui/geojson.py b/cabot_ui/cabot_ui/geojson.py index 111b0732..192d9c38 100644 --- a/cabot_ui/cabot_ui/geojson.py +++ b/cabot_ui/cabot_ui/geojson.py @@ -617,6 +617,7 @@ def set_target(self, link): gpoint = link.geometry.nearest_point_on_line(self.node.geometry) lpoint = geoutil.global2local(gpoint, self.anchor) self.update_pose(lpoint, link.pose.r + math.pi) + return gpoint def approaching_statement(self): return None diff --git a/cabot_ui/cabot_ui/navigation.py b/cabot_ui/cabot_ui/navigation.py index 0ec2859f..c88ec779 100644 --- a/cabot_ui/cabot_ui/navigation.py +++ b/cabot_ui/cabot_ui/navigation.py @@ -581,6 +581,7 @@ def _set_destination(self, destination): # check facilities self.nearby_facilities = [] links = list(filter(lambda x: isinstance(x, geojson.RouteLink), groute)) + target = groute[-1] if len(links) > 0: kdtree = geojson.LinkKDTree() kdtree.build(links) @@ -596,13 +597,18 @@ def _set_destination(self, destination): for ent in facility.entrances: min_link, min_dist = kdtree.get_nearest_link(ent.node) - if min_link and min_dist < 5.0: - # self._logger.debug(f"Facility - Link {facility._id}, {min_dist}, {facility.name}:{ent.name}, {min_link._id}") - ent.set_target(min_link) - self.nearby_facilities.append({ - "facility": facility, - "entrance": ent - }) + if min_link is None or min_dist >= 5.0: + continue + # self._logger.debug(f"Facility - Link {facility._id}, {min_dist}, {facility.name}:{ent.name}, {min_link._id}") + gp = ent.set_target(min_link) + dist = target.geometry.distance_to(gp) + if dist < 1.0: + self._logger.info(f"{facility._id=}: {facility.name=} is close to the node {target._id=}, {dist=}") + continue + self.nearby_facilities.append({ + "facility": facility, + "entrance": ent + }) end = time.time() self._logger.info(F"Check Facilities {end - start:.3f}.sec") @@ -862,7 +868,7 @@ def _check_info_poi(self, current_pose): def _check_nearby_facility(self, current_pose): if not self.nearby_facilities: return - entry = min(self.nearby_facilities, key=lambda p, c=current_pose: p["entrance"].distance_to(c)) + entry = min(self.nearby_facilities, key=lambda p, c=current_pose: abs(p["entrance"].distance_to(c))) if entry is None: return entrance = entry["entrance"]