Merge remote-tracking branch 'origin/summoning_planning_with_parking' into summoning_integration

Author: udymd

GEMstack/knowledge/routes/summoning_roadgraph_highbay.json

@@ -179,7 +179,7 @@ def __init__(self, roadgraphfn : str = None, map_type : str = 'roadgraph', map_f
         self.parking_route_existed = False
 
         # Used as route searchers' time limit as well as the update rate of the component
-        self.update_rate = 0.25
+        self.update_rate = 0.5
 
     def state_inputs(self):
         return ["all"]

GEMstack/onboard/planning/route_planning_component.py

@@ -111,8 +111,8 @@ def create_straight_lane(left_back : Tuple[float,float,float], left_forward : Tu
     lane.right = RoadgraphCurve(type=RoadgraphCurveEnum.LANE_BOUNDARY, segments=right_boundary_points)
 
     if begin_left is not None and begin_right is not None:
-        begin_boundary_points = segment_straight_line(begin_left, begin_right, resolution=resolution)
-        lane.begin = RoadgraphCurve(type=RoadgraphCurveEnum.LANE_BOUNDARY, segments=begin_boundary_points)
+        begin_boundary_points = segment_straight_line(begin_left, begin_right, resolution=resolution, crossable=crossable)
+        lane.begin = RoadgraphCurve(type=RoadgraphCurveEnum.LANE_BOUNDARY, segments=begin_boundary_points, crossable=crossable)
     if end_right is not None and end_left is not None:
         end_boundary_points = segment_straight_line(end_right, end_left, resolution=resolution)
         lane.end = RoadgraphCurve(type=RoadgraphCurveEnum.LANE_BOUNDARY, segments=end_boundary_points)
@@ -133,8 +133,8 @@ def create_arc_lane(left_back : Tuple[float,float,float], left_forward : Tuple[f
     lane = RoadgraphLane()
     left_boundary_points = segment_arc(left_back, left_forward, left_radius, direction, resolution=resolution)
     right_boundary_points = segment_arc(right_back, right_forward, right_radius, direction, resolution=resolution)
-    lane.left = RoadgraphCurve(type=RoadgraphCurveEnum.LANE_BOUNDARY, segments=left_boundary_points)
-    lane.right = RoadgraphCurve(type=RoadgraphCurveEnum.LANE_BOUNDARY, segments=right_boundary_points)
+    lane.left = RoadgraphCurve(type=RoadgraphCurveEnum.LANE_BOUNDARY, segments=left_boundary_points, crossable=crossable)
+    lane.right = RoadgraphCurve(type=RoadgraphCurveEnum.LANE_BOUNDARY, segments=right_boundary_points, crossable=crossable)
 
     if begin_left is not None and begin_right is not None:
         begin_boundary_points = segment_straight_line(begin_left, begin_right, resolution=resolution)
@@ -192,7 +192,6 @@ def create_lane(left_back : Tuple, left_forward : Tuple,
 
 if __name__ == '__main__':
     resolution = 0.4
-
     filename = 'GEMstack/knowledge/routes/summoning_roadgraph_sim.json'
     frame = ObjectFrameEnum.START
     roadgraph = Roadgraph(frame=frame)
@@ -306,6 +305,7 @@ def create_lane(left_back : Tuple, left_forward : Tuple,
 
     lon_ratio = (-88.235527 + 88.236129) / 51.34  # lon / m
     lat_ratio = (40.092819 - 40.092741) / 8.66    # lat / m
+    resolution = 0.4 * min(lon_ratio, lat_ratio)
 
     # Create lane segments
     roadgraph.lanes['highbay_outer_lane'] = create_straight_lane(left_back=(-88.236129, 40.092741 + lat_ratio * 1.5, 0.0), left_forward=(-88.235527, 40.092741 + lat_ratio * 1.5, 0.0),

roadgraph_generator.py

@@ -26,6 +26,8 @@ def get_lane_points_from_roadgraph(roadgraph: Roadgraph) -> List:
 
     roadgraphfn = "GEMstack/knowledge/routes/summoning_roadgraph_sim.json"
     map_frame = 'start'
+    # roadgraphfn = "GEMstack/knowledge/routes/summoning_roadgraph_highbay.json"
+    # map_frame = 'global'
     base, ext = os.path.splitext(roadgraphfn)
     if ext in ['.json', '.yml', '.yaml']:
         with open(roadgraphfn, 'r') as f:
@@ -43,11 +45,11 @@ def get_lane_points_from_roadgraph(roadgraph: Roadgraph) -> List:
     elif map_type == 'pointlist':
         lane_points = roadgraph.points
 
-    np.savetxt('roadgraph_lane_points.txt', lane_points, delimiter=',')
+    # np.savetxt('roadgraph_lane_points.txt', lane_points, delimiter=',')
 
     lane_points = np.array(lane_points)
-    x = lane_points[:, 0]
-    y = lane_points[:, 1]
+    x = lane_points[:, 0] #*100
+    y = lane_points[:, 1] #*100
     plt.scatter(x,y)
     plt.axis('equal')
     plt.show()