Add crosswalk link (#86)

* ADD Crosswalk link Signed-off-by: Shunsuke Kimura <[email protected]> * Add RouteStruct class Signed-off-by: Shunsuke Kimura <[email protected]> * Modify default value of rt_struct Signed-off-by: Shunsuke Kimura <[email protected]> * Add Copyright Signed-off-by: Shunsuke Kimura <[email protected]> * Fix copyright year Signed-off-by: Shunsuke Kimura <[email protected]> --------- Signed-off-by: Shunsuke Kimura <[email protected]>
CMU-cabot · Nov 20, 2024 · 28d2771 · 28d2771
1 parent 382db34
commit 28d2771
Show file tree

Hide file tree

Showing 2 changed files with 58 additions and 5 deletions.
diff --git 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
@@ -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