diff --git a/cabot_ui/cabot_ui/geojson.py b/cabot_ui/cabot_ui/geojson.py index 192d9c38..054fc697 100644 --- a/cabot_ui/cabot_ui/geojson.py +++ b/cabot_ui/cabot_ui/geojson.py @@ -1,4 +1,4 @@ -# Copyright (c) 2020, 2022 Carnegie Mellon University +# Copyright (c) 2020, 2024 Carnegie Mellon University and Miraikan # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal @@ -43,6 +43,20 @@ from cabot_ui.cabot_rclpy_util import CaBotRclpyUtil +class RouteStruct(enum.Enum): + """ Route Struct Class """ + """ See p.14 of https://www.mlit.go.jp/common/001244374.pdf """ + Sidewalk = 1 + SidewalkNotSeparatedFromRoadway = 2 + Crosswalk = 3 + CrosswalkWithoutSurfaceMarking = 4 + Underpass = 5 + PedestrianBridge = 6 + InFacility = 7 + OtherRouteStruct = 8 + Unknown = 99 + + class Geometry(object): """Geometry class""" @@ -145,7 +159,8 @@ def marshal(cls, dic): "hulop_tags": None, "hulop_poi_external_category": None, "hulop_show_labels_zoomlevel": None, - "hulop_road_width": 1.5 + "hulop_road_width": 1.5, + "rt_struct": RouteStruct.InFacility } def __getattr__(self, name): @@ -412,13 +427,16 @@ class NavigationMode(enum.Enum): Standard = 0 Narrow = 1 Tight = 2 + Crosswalk = 5 @classmethod - def get_mode(cls, width): + def get_mode(cls, width, rt_struct): if width < 1.0: return NavigationMode.Tight if width < 1.2: return NavigationMode.Narrow + if rt_struct == RouteStruct.Crosswalk or rt_struct == RouteStruct.CrosswalkWithoutSurfaceMarking: + return NavigationMode.Crosswalk return NavigationMode.Standard @@ -485,7 +503,10 @@ def is_leaf(self): @property def navigation_mode(self): - return NavigationMode.get_mode(self.properties.hulop_road_width) + return NavigationMode.get_mode( + width=self.properties.hulop_road_width, + rt_struct=self.properties.rt_struct + ) @property def length(self): diff --git a/cabot_ui/cabot_ui/navgoal.py b/cabot_ui/cabot_ui/navgoal.py index 75dcf5f4..a4e70c2c 100644 --- a/cabot_ui/cabot_ui/navgoal.py +++ b/cabot_ui/cabot_ui/navgoal.py @@ -1,4 +1,4 @@ -# Copyright (c) 2022 Carnegie Mellon University +# Copyright (c) 2022, 2024 Carnegie Mellon University and Miraikan # # Permission is hereby granted, free of charge, to any person obtaining a copy # of this software and associated documentation files (the "Software"), to deal @@ -699,6 +699,36 @@ def get_parameters_for(cls, mode): complete_stop: [false,false,true,false,true,false,true] /cabot/speed_control_node_touch_false: complete_stop: [false,false,true,false,true,false,true] +""" + if mode == geojson.NavigationMode.Crosswalk: + params = """ +/planner_server: + CaBot.path_adjusted_center: 0.0 + CaBot.path_adjusted_minimum_path_width: 0.0 + CaBot.path_width: 0.0 + CaBot.min_iteration_count: 5 + CaBot.max_iteration_count: 10 + CaBot.ignore_people: True +/footprint_publisher: + footprint_mode: 3 +/controller_server: + FollowPath.max_vel_x: 1.0 + FollowPath.sim_time: 0.5 + cabot_goal_checker.xy_goal_tolerance: 0.1 +/global_costmap/global_costmap: + people_obstacle_layer.people_enabled: False + inflation_layer.inflation_radius: 0.45 +/local_costmap/local_costmap: + inflation_layer.inflation_radius: 0.45 +/cabot/lidar_speed_control_node: + min_distance: 0.60 +/cabot/people_speed_control_node: + social_distance_x: 1.0 + social_distance_y: 0.50 +/cabot/speed_control_node_touch_true: + complete_stop: [false,false,true,false,true,false,true] +/cabot/speed_control_node_touch_false: + complete_stop: [false,false,true,false,true,false,true] """ data = yaml.safe_load(params) return data @@ -867,6 +897,8 @@ def done_callback(self, future): if status == GoalStatus.STATUS_SUCCEEDED: if self.mode == geojson.NavigationMode.Narrow or self.mode == geojson.NavigationMode.Tight: self.delegate.activity_log("cabot/navigation", "goal_completed", "NarrowGoal") + if self.mode == geojson.NavigationMode.Crosswalk: + self.delegate.activity_log("cabot/navigation", "goal_completed", "CrosswalkGoal") if status == GoalStatus.STATUS_SUCCEEDED and self.route_index + 1 < len(self.navcog_routes): self.route_index += 1