[waypoints]

look for the active waypoint in the graph using nearestNode3D

data/graphics/palettes/bluesky-default

@@ -4,7 +4,7 @@ aptsymbol=(148, 178, 235)
 coastlines=(85, 85, 115)
 conflict=(255, 160, 0)
 pavement=(160, 160, 160)
-polys=(0,255,255)
+polys=(255,0,0)
 previewpoly=(0, 204, 255)
 route=(255, 0, 255)
 runways=(100, 100, 100)

plugins/usepe_plugin.py

@@ -54,8 +54,8 @@ def init_plugin():
 
     # ---------------------------------- DEFINED BY USER ------------------------------------
     # config_path = r"C:\workspace3\scenarios-USEPE\scenario\USEPE\exercise_1\settings_exercise_1_reference.cfg"
-    config_path = r"C:\workspace3\scenarios-USEPE\scenario\USEPE\test\settings_OSD_3.cfg"
-    # config_path = r"C:\workspace3\scenarios-USEPE\scenario\USEPE\OSD\settings_OSD_3.cfg"
+    # config_path = r"C:\workspace3\scenarios-USEPE\scenario\USEPE\test\settings_OSD_3.cfg"
+    config_path = r"C:\workspace3\scenarios-USEPE\scenario\USEPE\OSD\settings_OSD_3.cfg"
     # config_path = r"/home/ror/ws/scenarios/scenario/USEPE/exercise_3/settings_exe_3_ref.cfg"
     # ------------------------------------------------------------------------------------------
 
@@ -70,15 +70,15 @@ def init_plugin():
     usepeconfig.read( config_path )
 
     graph_path = usepeconfig['BlueSky']['graph_path']
-    segment_path = usepeconfig['BlueSky']['segment_path']
+    # segment_path = usepeconfig['BlueSky']['segment_path']
     flight_plan_csv_path = usepeconfig['BlueSky']['flight_plan_csv_path']
 
     initial_time = int( usepeconfig['BlueSky']['initial_time'] )
     final_time = int( usepeconfig['BlueSky']['final_time'] )
 
     usepegraph = UsepeGraph( graph_path )
     # print( ' - Graph initialised - ' )
-    usepesegments = UsepeSegments( segment_path )
+    usepesegments = UsepeSegments()
     # print( ' - Segments initialised - ' )
     usepestrategic = UsepeStrategicDeconfliction( initial_time, final_time )
     # print( ' - Strategic deconfliction initialised - ' )
@@ -212,7 +212,7 @@ class UsepeSegments( core.Entity ):
     iii) routes in the strategic phase
     '''
 
-    def __init__( self, segment_path ):
+    def __init__( self ):
         super().__init__()
 
         # with open( segment_path, 'rb' ) as f:
@@ -361,18 +361,42 @@ def calcRecentPath( self ):
 
             acrte = traf.ap.route[i]
             iactwp = acrte.iactwp
+            # actwpt = acrte.wpname[iactwp]
+            # wpt_dict = self.wpt_dict[traf.id[i]]
+            if acrte.wpalt[iactwp] < 0:
+                actwpt_alt = traf.ap.alt[i]
+            else:
+                actwpt_alt = acrte.wpalt[iactwp]
 
-            wpt_dict = self.wpt_dict[traf.id[i]]
+            actwpt = nearestNode3d( usepegraph.graph,
+                                    acrte.wplon[iactwp],
+                                    acrte.wplat[iactwp],
+                                    actwpt_alt,
+                                    exclude_corridor=False )
 
-            if list( wpt_dict.keys() ).index( acrte.wpname[iactwp] ) == 0:
-                prev_actwpt = acrte.wpname[iactwp]
+            if iactwp == 0:
+                i_prevactwpt = iactwp
             else:
-                prev_actwpt = list( wpt_dict.keys() )[list( wpt_dict.keys() ).index( acrte.wpname[iactwp] ) - 1]
-                # prev_actwpt = acrte.wpname[iactwp - 1]
+                i_prevactwpt = iactwp - 1
+                # prev_actwpt = list( wpt_dict.keys() )[list( wpt_dict.keys() ).index( acrte.wpname[iactwp] ) - 1]
+            if acrte.wpalt[i_prevactwpt] < 0:
+                actwpt_alt = traf.ap.alt[i]
+            else:
+                actwpt_alt = acrte.wpalt[i_prevactwpt]
+            prev_actwpt = nearestNode3d( usepegraph.graph,
+                                         acrte.wplon[i_prevactwpt],
+                                         acrte.wplat[i_prevactwpt],
+                                         acrte.wpalt[i_prevactwpt],
+                                         exclude_corridor=False )
 
             temparr = np.empty_like( self.recentpath[i] )
+            #=======================================================================================
+            # currentpos = ( sim.simt, traf.lat[i], traf.lon[i], traf.alt[i],
+            #                wpt_dict[acrte.wpname[iactwp]], wpt_dict[prev_actwpt] )
+            #=======================================================================================
+
             currentpos = ( sim.simt, traf.lat[i], traf.lon[i], traf.alt[i],
-                           wpt_dict[acrte.wpname[iactwp]], wpt_dict[prev_actwpt] )
+                           actwpt, prev_actwpt )
             temparr[-1] = currentpos
             temparr[:-1] = self.recentpath[i][1:]
             self.recentpath[i][:] = temparr
@@ -909,10 +933,12 @@ def processFlightPlans( self ):
             if ( segment_name_0 == 'N/A' ) | ( segment_name_f == 'N/A' ):
                 # origin or destination is not within any segment
                 self.flight_plan_df_buffer = self.flight_plan_df_buffer.drop( self.flight_plan_df_buffer.index[0] )
+                print( 'Origin or destination is not within any segment' )
             else:
                 if ( usepesegments.segments['speed_max'][segment_name_0] == 0 ) | ( usepesegments.segments['speed_max'][segment_name_f] == 0 ):
                     # origin or destination is not allowed, so the flight plan is rejected
                     self.flight_plan_df_buffer = self.flight_plan_df_buffer.drop( self.flight_plan_df_buffer.index[0] )
+                    print( 'Origin or destination is not allowed, so the flight plan is rejected' )
                 else:
                     usepestrategic.strategicDeconflictionDrone( df_row )
                     self.flight_plan_df_buffer = self.flight_plan_df_buffer.drop( self.flight_plan_df_buffer.index[0] )

usepe/city_model/utils.py

@@ -82,8 +82,7 @@ def layersDict( config ):
 
     return layers_dict
 
-
-def nearestNode3d( G, lon, lat, altitude ):
+def nearestNode3d( G, lon, lat, altitude, exclude_corridor=True ):
     '''
     This function gets the closest node of the city graph nodes with respect
     to a given reference point (lat, lon, alt)
@@ -100,7 +99,10 @@ def nearestNode3d( G, lon, lat, altitude ):
     '''
     # The nodes are filtered to exclude corridor nodes
     nodes = list( G.nodes )
-    filtered_latlon = list( filter( lambda node: str( node )[:3] != 'COR', nodes ) )
+    if exclude_corridor:
+        filtered_latlon = list( filter( lambda node: str( node )[:3] != 'COR', nodes ) )
+    else:
+        filtered_latlon = nodes
     # Iterates to get the closest one
     nearest_node = filtered_latlon[0]
     delta_xyz = ( ( G.nodes[nearest_node]['z'] - altitude ) ** 2 +