search_for_calc.h

最新推荐文章于 2018-10-19 23:58:46 发布
原创 最新推荐文章于 2018-10-19 23:58:46 发布 · 454 阅读 · 0 ·
CC 4.0 BY-SA版权
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
文章标签：

#search #callback

本文介绍了Windows应用程序中消息处理函数WndProc和特定资源ID的宏定义，包括退出、测试及关于对话框的处理。通过宏定义实现了资源标识符的指定，并展示了两个回调函数的声明。

  name="google_ads_frame" marginwidth="0" marginheight="0" src="http://pagead2.googlesyndication.com/pagead/ads?client=ca-pub-5572165936844014&dt=1194442938015&lmt=1194190197&format=336x280_as&output=html&correlator=1194442937843&url=file%3A%2F%2F%2FC%3A%2FDocuments%2520and%2520Settings%2Flhh1%2F%E6%A1%8C%E9%9D%A2%2FCLanguage.htm&color_bg=FFFFFF&color_text=000000&color_link=000000&color_url=FFFFFF&color_border=FFFFFF&ad_type=text&ga_vid=583001034.1194442938&ga_sid=1194442938&ga_hid=1942779085&flash=9&u_h=768&u_w=1024&u_ah=740&u_aw=1024&u_cd=32&u_tz=480&u_java=true" frameborder="0" width="336" scrolling="no" height="280" allowtransparency="allowtransparency"> #define IDM_EXIT           100
#define IDM_TEST           200
#define IDM_ABOUT          301

LRESULT CALLBACK WndProc  (HWND, UINT, WPARAM, LPARAM);
LRESULT CALLBACK About    (HWND, UINT, WPARAM, LPARAM);

AMCL系列之头文件map.h
qq_34160473的博客
03-08 321
AMCL的地图头文件
map.h C++ 头文件下载
11-24
map.h C++ 头文件下载 C++17 标准库文件 unordered_map map
WXAPP-WEB-ShowMess:采用云开发后端服务打造的实时消息展板程序，使用小程序的客服消息能力发送消息，实时在WEB展板页面中展示，用于现场互动等多种玩法场景
05-28
实时消息展板——腾讯云·云开发Techo大会演示项目 项目介绍 采用云开发后端服务打造的实时消息展板程序。 使用小程序的客服消息能力发送消息，实时在WEB展板页面中展示，用于现场互动等多种玩法场景。 演示地址： 部署步骤 注意：一键部署之后还需要操作下述步骤中第5步 在部署成功之后，控制台会显示访问按钮，点击即可进入WEB页面应用 小程序访问需要微信搜索「小程序助手」，进入后选择以上部署的小程序-「版本管理」-「开发版」中会有CI机器人提交的最新版本，点击之后即可进入小程序 小程序开发者工具打开此项目 确定云开发环境（按量付费）最好有一个全新的按量付费小程序 将项目下载后打开，cloudfunctions存放云函数代码，miniprogram存放小程序代码，webview存放WEB应用代码 将cloudfunctions目录下三个云函数创建上传并云端安装依赖。delete云函数需要上传触发
2_menus.h
CCodes的专栏
11-08 664
  #define IDM_EXIT           100#define IDM_OLD            200#define IDM_NEW            201#define IDM_ABOUT          301LRESULT CALLBACK WndProc  (HWND, UINT, WPARAM, LPARAM);LRESULT CALLBACK Abou
《STL源码剖析》-- stl_map.h
一个人的战争
08-03 6256
// Filename: stl_map.h // Comment By: 凝霜 // E-mail: mdl2009@vip.qq.com // Blog: http://blog.csdn.net/mdl13412 /* * * Cop
make: Circular mod_prec.o <- mod_prec.o dependency dropped. gfortran -O3 -ffixed-line-length-none -ffree-line-length-0 -fallow-argument-mismatch func_pointer.o mod_prec.o sinter.o mod_types.o mod_time.o mod_main.o mod_spherical.o mod_utils.o mod_clock.o eqs_of_state.o mod_interp.o mod_par.o mod_par_special.o mod_ncll.o mod_nctools.o mod_wd.o mod_sng.o mod_heatflux.o mod_solar.o mod_bulk.o mod_input.o mod_force.o mod_obcs.o mod_petsc.o mod_semi_implicit.o mod_non_hydro.o mod_set_time.o ice_kinds_mod.o ice_model_size.o ice_domain.o ice_constants.o ice_fileunits.o ice_state.o ice_work.o ice_grid.o ice_albedo.o ice_calendar.o ice_flux.o ice_flux_in.o ice_itd.o ice_mechred.o ice_itd_linear.o ice_scaling.o ice_atmo.o ice_ocean.o ice_therm_vertical.o ice_init.o ice_therm_itd.o mod_ice2d.o mod_ice.o mod_startup.o mod_wqm.o mod_ncdio.o mod_setup.o mod_newinp.o particle.o linklist.o mod_lag.o mod_northpole.o mod_pwp.o mod_dye.o mod_optimal_interpolation.o mod_report.o mod_probe.o mod_gotm.o mod_balance_2d.o mod_tridiag.o mod_scal.o mod_meanflow.o mod_obcs2.o mod_obcs3.o mod_sed.o mod_enkf.o mod_etkf.o mod_rrk.o mod_rrkf_obs.o mod_rrkassim.o mod_enkf_ncd.o enkf_ncdio.o mod_enkf_obs.o mod_enkfassim.o mod_assim.o mod_nesting.o mod_visit.o mod_plbc.o mod_dam.o mod_station_timeseries.o mod_sparse_timeseries.o mod_boundschk.o mod_esmf_nesting.o mod_cstms_vars.o mod_flocmod.o mod_sed_cstms.o mod_fluid_mud.o mod_tvd.o mod_mld_rho.o mod_vegetation.o mod_heatflux_sediment.o mod_vector_projection.o mod_main_wave.o swmod1.o swmod2.o swmod3.o mod_action_im.o mod_action_ex.o mod_wavesetup.o mod_wave_current_interaction.o mod_bbl.o fvcom.o genmap.o tge.o longshore_flow.o cell_area.o open_all.o load_grid.o allocate_all.o setup_domain.o genmap_obc.o genmap_lsf.o print_vals.o coords_n_const.o shape_coef_gcn.o shape_coef_gcy.o depth_grad.o grid_metrics.o cntrl_prmtrs.o init_sed.o internal_step.o bcond_gcn.o bcond_gcy.o adjust2d3d.o brough.o advection_edge_gcn.o advection_edge_gcy.o ghostuv.o advave_edge_gcn.o advave_edge_gcy.o phy_baropg.o baropg.o external_step.o extel_edge.o extuv_edge.o depth_check.o vertvl_edge.o adv_uv_edge_gcn.o adv_uv_edge_gcy.o vdif_uv.o extelpf_edge.o wreal.o viscofh.o adv_q.o fct_q2.o fct_q2l.o vdif_q.o adv_t.o adv_s.o fct_t.o vdif_ts.o fct_s.o bcond_ts.o adjust_ts.o conv_over.o visitsim.o startup_type.o edge_len.o adcor.o icing.o rho_pmean.o calc_vort.o namelist.o nh_set_nesting.o coare26z.o coare40vn.o adv_uv_edge_gcn_rk.o adv_uv_edge_gcy_rk.o adv_t_rk.o adv_s_rk.o adv_q_rk.o mod_bio_3D.o mod_onedtide.o ocpmix.o ocpcre.o ocpids.o swanpre1.o swanpre2.o swanser.o swanmain.o swancom1.o swancom2.o swancom3.o swancom4.o swancom5.o w3part.o -L/usr/local/lib -lmetis -lnetcdff -lnetcdf -L/home/fvcom/FVCOM-5.1.0/src/libs/install/lib -ljulian -Wl,-rpath,/home/fvcom/FVCOM-5.1.0/petsc/arch-linux-c-debug/lib -L/home/fvcom/FVCOM-5.1.0/petsc/arch-linux-c-debug/lib -Wl,-rpath,/usr/lib/x86_64-linux-gnu/openmpi/lib/fortran/gfortran -L/usr/lib/x86_64-linux-gnu/openmpi/lib/fortran/gfortran -Wl,-rpath,/usr/lib/gcc/x86_64-linux-gnu/11 -L/usr/lib/gcc/x86_64-linux-gnu/11 -lpetsc -lHYPRE -lflapack -lfblas -lm -lmpi_usempif08 -lmpi_usempi_ignore_tkr -lmpi_mpifh -lmpi -lopen-rte -lopen-pal -lhwloc -levent_core -levent_pthreads -lgfortran -lm -lz -lgfortran -lm -lgfortran -lgcc_s -lquadmath -lstdc++ -lquadmath -o fvcom /usr/bin/ld: mod_par.o: in function `__mod_par_MOD_domdec': mod_par.f90:(.text+0x667aa): undefined reference to `partition_' collect2: error: ld returned 1 exit status make: *** [makefile:135: fvcom] Error 1
09-29
After a quick search in the FVCOM source code, I found that in the `fvcom-5.1.0` source, there is a file `partition_mesh.F` that contains a subroutine `partition`. This subroutine calls the METIS ...
""" A* grid planning author: Atsushi Sakai(@Atsushi_twi) Nikos Kanargias (nkana@tee.gr) See Wikipedia article (https://en.wikipedia.org/wiki/A*_search_algorithm) """ import math import matplotlib.pyplot as plt show_animation = True class AStarPlanner: def __init__(self, ox, oy, resolution, rr): """ Initialize grid map for a star planning ox: x position list of Obstacles [m] oy: y position list of Obstacles [m] resolution: grid resolution [m],地图的像素 rr: robot radius[m] """ self.resolution = resolution self.rr = rr self.min_x, self.min_y = 0, 0 self.max_x, self.max_y = 0, 0 self.obstacle_map = None self.x_width, self.y_width = 0, 0 self.motion = self.get_motion_model() self.calc_obstacle_map(ox, oy) class Node: """定义搜索区域节点类,每个Node都包含坐标x和y, 移动代价cost和父节点索引。 """ def __init__(self, x, y, cost, parent_index): self.x = x # index of grid self.y = y # index of grid self.cost = cost self.parent_index = parent_index def __str__(self): return str(self.x) + "," + str(self.y) + "," + str( self.cost) + "," + str(self.parent_index) def planning(self, sx, sy, gx, gy): """ A star path search 输入起始点和目标点的坐标(sx,sy)和(gx,gy)， 最终输出的结果是路径包含的点的坐标集合rx和ry。 input: s_x: start x position [m] s_y: start y position [m] gx: goal x position [m] gy: goal y position [m] output: rx: x position list of the final path ry: y position list of the final path """ start_node = self.Node(self.calc_xy_index(sx, self.min_x), self.calc_xy_index(sy, self.min_y), 0.0, -1) goal_node = self.Node(self.calc_xy_index(gx, self.min_x), self.calc_xy_index(gy, self.min_y), 0.0, -1) open_set, closed_set = dict(), dict() open_set[self.calc_grid_index(start_node)] = start_node while 1: if len(open_set) == 0: print("Open set is empty..") break c_id = min( open_set, key=lambda o: open_set[o].cost + self.calc_heuristic(goal_node, open_set[o])) current = open_set[c_id] # show graph if show_animation: # pragma: no cover plt.plot(self.calc_grid_position(current.x, self.min_x), self.calc_grid_position(current.y, self.min_y), "xc") # for stopping simulation with the esc key. plt.gcf().canvas.mpl_connect('key_release_event', lambda event: [exit(0) if event.key == 'escape' else None]) if len(closed_set.keys()) % 10 == 0: plt.pause(0.001) # 通过追踪当前位置current.x和current.y来动态展示路径寻找 if current.x == goal_node.x and current.y == goal_node.y: print("Find goal") goal_node.parent_index = current.parent_index goal_node.cost = current.cost break # Remove the item from the open set del open_set[c_id] # Add it to the closed set closed_set[c_id] = current # expand_grid search grid based on motion model for i, _ in enumerate(self.motion): node = self.Node(current.x + self.motion[i][0], current.y + self.motion[i][1], current.cost + self.motion[i][2], c_id) n_id = self.calc_grid_index(node) # If the node is not safe, do nothing if not self.verify_node(node): continue if n_id in closed_set: continue if n_id not in open_set: open_set[n_id] = node # discovered a new node else: if open_set[n_id].cost > node.cost: # This path is the best until now. record it open_set[n_id] = node rx, ry = self.calc_final_path(goal_node, closed_set) return rx, ry def calc_final_path(self, goal_node, closed_set): # generate final course rx, ry = [self.calc_grid_position(goal_node.x, self.min_x)], [ self.calc_grid_position(goal_node.y, self.min_y)] parent_index = goal_node.parent_index while parent_index != -1: n = closed_set[parent_index] rx.append(self.calc_grid_position(n.x, self.min_x)) ry.append(self.calc_grid_position(n.y, self.min_y)) parent_index = n.parent_index return rx, ry @staticmethod def calc_heuristic(n1, n2): """计算启发函数 Args: n1 (_type_): _description_ n2 (_type_): _description_ Returns: _type_: _description_ """ w = 1.0 # weight of heuristic d = w * math.hypot(n1.x - n2.x, n1.y - n2.y) return d def calc_grid_position(self, index, min_position): """ calc grid position :param index: :param min_position: :return: """ pos = index * self.resolution + min_position return pos def calc_xy_index(self, position, min_pos): return round((position - min_pos) / self.resolution) def calc_grid_index(self, node): return (node.y - self.min_y) * self.x_width + (node.x - self.min_x) def verify_node(self, node): px = self.calc_grid_position(node.x, self.min_x) py = self.calc_grid_position(node.y, self.min_y) if px < self.min_x: return False elif py < self.min_y: return False elif px >= self.max_x: return False elif py >= self.max_y: return False # collision check if self.obstacle_map[node.x][node.y]: return False return True def calc_obstacle_map(self, ox, oy): self.min_x = round(min(ox)) self.min_y = round(min(oy)) self.max_x = round(max(ox)) self.max_y = round(max(oy)) print("min_x:", self.min_x) print("min_y:", self.min_y) print("max_x:", self.max_x) print("max_y:", self.max_y) self.x_width = round((self.max_x - self.min_x) / self.resolution) self.y_width = round((self.max_y - self.min_y) / self.resolution) print("x_width:", self.x_width) print("y_width:", self.y_width) # obstacle map generation self.obstacle_map = [[False for _ in range(self.y_width)] for _ in range(self.x_width)] for ix in range(self.x_width): x = self.calc_grid_position(ix, self.min_x) for iy in range(self.y_width): y = self.calc_grid_position(iy, self.min_y) for iox, ioy in zip(ox, oy): d = math.hypot(iox - x, ioy - y) if d <= self.rr: self.obstacle_map[ix][iy] = True break @staticmethod def get_motion_model(): # dx, dy, cost motion = [[1, 0, 1], [0, 1, 1], [-1, 0, 1], [0, -1, 1], [-1, -1, math.sqrt(2)], [-1, 1, math.sqrt(2)], [1, -1, math.sqrt(2)], [1, 1, math.sqrt(2)]] return motion def main(): print(__file__ + " start!!") # start and goal position sx = 10.0 # [m] sy = 10.0 # [m] gx = 50.0 # [m] gy = 50.0 # [m] grid_size = 2.0 # [m] robot_radius = 1.0 # [m] # set obstacle positions ox, oy = [], [] for i in range(-10, 60): ox.append(i) oy.append(-10.0) for i in range(-10, 60): ox.append(60.0) oy.append(i) for i in range(-10, 61): ox.append(i) oy.append(60.0) for i in range(-10, 61): ox.append(-10.0) oy.append(i) for i in range(-10, 40): ox.append(20.0) oy.append(i) for i in range(0, 40): ox.append(40.0) oy.append(60.0 - i) if show_animation: # pragma: no cover plt.plot(ox, oy, ".k") plt.plot(sx, sy, "og") plt.plot(gx, gy, "xb") plt.grid(True) plt.axis("equal") a_star = AStarPlanner(ox, oy, grid_size, robot_radius) rx, ry = a_star.planning(sx, sy, gx, gy) if show_animation: # pragma: no cover plt.plot(rx, ry, "-r") plt.pause(0.001) plt.show() if __name__ == '__main__': main() 详细讲解一下上面这段代码是如何实现功能的
最新发布
11-14
我们将定义节点类（Node）来存储每个节点的信息，包括坐标、父节点、g值、h值、f值（通过g和h计算得到）。 然后，我们实现A*算法的主函数。 注意：我们假设网格是二维数组，每个位置可以是0（可通行）或1（障碍物...
import pandas as pd import networkx as nx import numpy as np import itertools # --- Constants and Assumptions --- # These should be clearly stated and can be modified. VOLTAGE_KV = 10.0 # Line voltage in kV ROOT_3 = np.sqrt(3) BASE_DG_CAPACITY_KW = 300.0 # Initial capacity for each DG N_DG = 8 # Failure rates from problem description FAILURE_RATE_DG_PERCENT = 0.5 / 100.0 # FAILURE_RATE_USER_PERCENT = 0.5 / 100.0 # Not directly used in this simplified line-fault model for widespread outages # FAILURE_RATE_SWITCH_PERCENT = 0.2 / 100.0 # Assuming switch failures manifest as line failures or inability to operate tie FAILURE_RATE_LINE_PER_KM = 0.002 # Per km per year (assuming rates are annual) # Costs (placeholders - these are critical for actual risk values) # Value of Lost Load (VoLL) in monetary units per kW per hour. # For risk = P * C, if P is annual probability, C should be impact of one event. # Let's define C_loss as total kW unserved * a severity factor. # Or, if we want an annual risk cost: P_annual_fault * kW_unserved * hours_outage * cost_per_kWh # For simplicity, using $/kW of unserved load for the consequence C. COST_VOLL_PER_KW = 10.0 # Example: $10 per kW of unserved load AVG_OUTAGE_DURATION_H = 4 # Example: average hours for an outage, if converting to energy # Cost of Overload (Consequence C_over) # This can be complex: accelerated aging, tripping, damage. # Simplified: A penalty if any line is overloaded in a given state. COST_PENALTY_FOR_ANY_OVERLOAD = 1000.0 # Example: $1000 penalty if system is in an overloaded state # Or, a cost per MWh of overloaded energy, or per overloaded line. # Line and Feeder Capacities # Main feeder rated current from problem: 220A. # P_rated_feeder_kW = ROOT_3 * VOLTAGE_KV * FEEDER_RATED_CURRENT_A * 1.0 (pf=1) # = 1.732 * 10 * 220 = 3810.4 kW (approx 3.8 MW, problem says 2.2MW for 220A, implies lower pf or different basis) # Let's use current as the primary limit. FEEDER_RATED_CURRENT_A = 220.0 # Assumption for individual line segments: For this model, we'll assume all lines # have a rated current equal to the main feeder. This is a strong simplification. # A more detailed model would assign ratings based on conductor types or downstream load. LINE_RATED_CURRENT_A = 100.0 # More conservative assumption for individual segments than 220A. Needs proper engineering values. # For lines directly from substation, perhaps 220A is more appropriate. # Let's use a dictionary for specific line ratings if known, else default. DEFAULT_LINE_RATED_CURRENT_A = 100.0 # Tie Line Capacity TIE_LINE_RATED_CURRENT_A = 150.0 # Assumption, should be based on tie switch/line capacity # DG Locations (Node IDs from 1 to 62) - Based on Figure 1 interpretation DG_LOCATIONS_KW = { 6: BASE_DG_CAPACITY_KW, 10: BASE_DG_CAPACITY_KW, 15: BASE_DG_CAPACITY_KW, 27: BASE_DG_CAPACITY_KW, 31: BASE_DG_CAPACITY_KW, 37: BASE_DG_CAPACITY_KW, 50: BASE_DG_CAPACITY_KW, 58: BASE_DG_CAPACITY_KW } # Tie Switches: (node1, node2, switch_id_text) - normally open # Interpretation based on careful review of Figure 1: # S13-1: (13, 22) - Intra-Feeder 1 (Connects two branches of Feeder 1) # S29-2: (29, 42) - Intra-Feeder 2 (Connects two branches of Feeder 2) # S62-3: (62, 19) - Inter-Feeder (Connects Feeder 3 (node 62) to Feeder 1 (node 19)) TIE_SWITCHES_INFO = [ {'nodes': (13, 22), 'id': 'S13-1', 'type': 'intra-F1', 'capacity_A': TIE_LINE_RATED_CURRENT_A}, {'nodes': (29, 42), 'id': 'S29-2', 'type': 'intra-F2', 'capacity_A': TIE_LINE_RATED_CURRENT_A}, {'nodes': (62, 19), 'id': 'S62-3', 'type': 'inter-F3_F1', 'capacity_A': TIE_LINE_RATED_CURRENT_A} ] # This interpretation means Feeder 2 cannot directly receive support from F1 or F3. # If problem implies all feeders can support each other, TIE_SWITCHES_INFO would need redefinition. # Substation connection points (source nodes for feeders) # CB1 -> Node 1, CB2 -> Node 23, CB3 -> Node 43 # Node 0 will represent the main grid / infinite source. SOURCE_NODE = 0 SUBSTATION_CONNECTIONS = { 'CB1': (SOURCE_NODE, 1), 'CB2': (SOURCE_NODE, 23), 'CB3': (SOURCE_NODE, 43) } # Capacity of connection from source to substation nodes (effectively feeder capacity) SUBSTATION_LINE_CAPACITY_A = FEEDER_RATED_CURRENT_A # --- Data Loading Functions --- def load_load_data(filename="C题附件：有源配电网62节点系统基本参数.xlsx - 表1 有源配电网62节点系统负荷参数.csv"): df = pd.read_csv(filename) df.columns = ['node_id', 'load_kw'] # Convert node_id to int if it's not already df['node_id'] = df['node_id'].astype(int) return df.set_index('node_id')['load_kw'].to_dict() def load_topology_data(filename="C题附件：有源配电网62节点系统基本参数.xlsx - 表2 有源配电网62节点系统拓扑参数.csv"): df = pd.read_csv(filename) # Rename columns for easier access (assuming standard Chinese headers) df.columns = ['line_num', 'from_node', 'to_node', 'length_km', 'resistance_ohm', 'reactance_ohm'] # Convert relevant columns to numeric for col in ['from_node', 'to_node', 'length_km', 'resistance_ohm', 'reactance_ohm']: df[col] = pd.to_numeric(df[col], errors='coerce') return df # --- Core Power Grid Model Class --- class PowerGridModel: def __init__(self, load_data, topology_data, dg_locations_kw, tie_switches_info, substation_connections): self.loads_kw = load_data self.topology_df = topology_data self.dg_kw = dg_locations_kw.copy() # Allow modification for different scenarios self.tie_switches_info = tie_switches_info self.substation_connections = substation_connections self.graph = self._build_graph() self.feeder_info = self._identify_feeders() def _build_graph(self): G = nx.Graph() # Use Graph for undirected, or DiGraph if flow direction is fixed by sources # Add nodes with load and DG info all_nodes = set(self.topology_df['from_node']) | set(self.topology_df['to_node']) for node_id in all_nodes: node_id = int(node_id) # Ensure int G.add_node(node_id, load_kw=self.loads_kw.get(node_id, 0), dg_kw=self.dg_kw.get(node_id, 0)) # Add lines from topology data for _, row in self.topology_df.iterrows(): u, v = int(row['from_node']), int(row['to_node']) G.add_edge(u, v, id=row['line_num'], length_km=row['length_km'], resistance_ohm=row['resistance_ohm'], # reactance_ohm=row['reactance_ohm'], # Ignoring reactance as per problem rated_current_a=DEFAULT_LINE_RATED_CURRENT_A, # Default, can be refined failed=False) # Add substation connections (virtual lines from a common source) # These represent the main feeder lines from CBs G.add_node(SOURCE_NODE, type='source') for cb_id, (src, dest_node) in self.substation_connections.items(): G.add_edge(src, dest_node, id=cb_id, length_km=0.01, resistance_ohm=0.001, # Minimal impedance rated_current_a=SUBSTATION_LINE_CAPACITY_A, type='substation_link', failed=False) return G def _get_subgraph_with_operational_lines(self, graph_to_copy, faulty_line_edge=None): """Creates a subgraph considering only non-failed lines and open tie switches.""" g_op = graph_to_copy.copy() # Remove failed lines lines_to_remove = [] if faulty_line_edge: # faulty_line_edge is (u,v) if g_op.has_edge(*faulty_line_edge): lines_to_remove.append(faulty_line_edge) for u, v, data in list(g_op.edges(data=True)): if data.get('failed', False): lines_to_remove.append((u,v)) g_op.remove_edges_from(lines_to_remove) # Normally, tie switches are open. For restoration, specific ones might be closed. # This base function assumes they are open unless explicitly handled by restoration logic. return g_op def _identify_feeders(self): """Identifies nodes belonging to each feeder under normal operation (tie switches open).""" g_normal = self._get_subgraph_with_operational_lines(self.graph) feeder_info = {} # {'CB1': {nodes}, 'CB2': {nodes}, ...} for cb_id, (src_node, start_node) in self.substation_connections.items(): if g_normal.has_node(start_node) and g_normal.has_node(src_node) and nx.has_path(g_normal, src_node, start_node): # Find all nodes reachable from start_node without passing through another substation's start_node # or the main source node again, after removing other substation links. temp_g = g_normal.copy() other_cb_links = [] for other_cb, (s,d) in self.substation_connections.items(): if other_cb != cb_id and temp_g.has_edge(s,d): other_cb_links.append((s,d)) temp_g.remove_edges_from(other_cb_links) if nx.has_path(temp_g, src_node, start_node): # All nodes in the component connected to start_node, excluding the source itself component_nodes = nx.node_connected_component(temp_g.subgraph( [n for n in temp_g.nodes if n != src_node or n == start_node] # Consider start_node part of feeder ), start_node) feeder_info[cb_id] = component_nodes else: # Should not happen if graph is built correctly feeder_info[cb_id] = {start_node} if start_node in g_normal else set() else: feeder_info[cb_id] = {start_node} if start_node in g_normal else set() return feeder_info def _calculate_line_current_kw(self, power_kw): """Calculates current (A) given power (kW) at VOLTAGE_KV (line-to-line).""" if VOLTAGE_KV <= 0: return float('inf') return abs(power_kw) / (ROOT_3 * VOLTAGE_KV * 1.0) # Assumed PF=1 for current calculation from P def _get_downstream_info(self, G, line_u, line_v, source_nodes_for_feeder): """ Calculates total load and DG power downstream of a directed line (u,v), assuming v is further from the source_node for this path. G: graph to operate on (can be a faulted graph) source_nodes_for_feeder: list of possible source nodes for the current connected component. """ # Temporarily make graph directed from source to loads to find downstream nodes # This is tricky if the graph is not purely radial or has loops after closing ties. # A simpler approach for radial sections: # Check connectivity from sources to line_v, if line_u is removed. # If line_v is disconnected from all sources when (u,v) is cut, then everything # in the component of line_v is downstream. # Create a copy of G to modify temp_g = G.copy() if not temp_g.has_edge(line_u, line_v): return [], 0, 0 # Line doesn't exist temp_g.remove_edge(line_u, line_v) downstream_nodes = set() # Check which side (u or v) is disconnected from the source(s) # Assume u is closer to source, v is further. # If v is still connected to a source, then (u,v) might be part of a loop or fed from elsewhere. # A robust way: find path from source to v. If (u,v) is on all paths, then v is downstream of u via this line. # For radial feeders (normal operation): # If we consider (u,v) where u is parent of v: component_of_v = set() q = [line_v] visited = {line_u, line_v} # Start by marking u as visited (as if coming from u) # If line_v is connected to any source_node without passing through line_u v_connected_to_source_alt_path = False for src in source_nodes_for_feeder: if nx.has_path(temp_g, src, line_v): v_connected_to_source_alt_path = True break if v_connected_to_source_alt_path: # (u,v) is part of a loop or v is fed from elsewhere # This simple downstream logic is insufficient for meshed networks. # For now, assume radial for this part of flow calculation. # A more complex flow calculation (Newton-Raphson) would be needed for meshed. # Given problem constraints, assume feeders are normally radial. pass # This line might not have a clear "downstream" if looped. # If v is disconnected from source when (u,v) is cut, then its component is downstream. # Check connectivity for v in temp_g (where (u,v) is removed) v_still_connected = any(nx.has_path(temp_g, src, line_v) for src in source_nodes_for_feeder if src in temp_g) if not v_still_connected: # v is now isolated from source, so its component is downstream of (u,v) component_of_v = nx.node_connected_component(temp_g, line_v) else: # v is still connected, means (u,v) might be redundant or complex. # Try to determine direction based on distance from source dist_u = float('inf') dist_v = float('inf') for src in source_nodes_for_feeder: if src not in G: continue if nx.has_path(G, src, line_u): dist_u = min(dist_u, nx.shortest_path_length(G, src, line_u)) if nx.has_path(G, src, line_v): dist_v = min(dist_v, nx.shortest_path_length(G, src, line_v)) if dist_v > dist_u : # v is downstream of u # Find component of v if (u,v) is removed and v is not connected to source g_temp_removed_edge = G.copy() g_temp_removed_edge.remove_edge(line_u,line_v) is_v_conn_to_src = False for src_node_feeder in source_nodes_for_feeder: if src_node_feeder in g_temp_removed_edge and nx.has_path(g_temp_removed_edge, src_node_feeder, line_v): is_v_conn_to_src = True break if not is_v_conn_to_src: component_of_v = nx.node_connected_component(g_temp_removed_edge, line_v) else: # v is still connected, (u,v) is likely a loop closing line. Flow is complex. # For simplicity, this function will return 0 flow for loop lines if direction is ambiguous. return [], 0, 0 elif dist_u > dist_v: # u is downstream of v (swap them) # similar logic for u g_temp_removed_edge = G.copy() g_temp_removed_edge.remove_edge(line_u,line_v) is_u_conn_to_src = False for src_node_feeder in source_nodes_for_feeder: if src_node_feeder in g_temp_removed_edge and nx.has_path(g_temp_removed_edge, src_node_feeder, line_u): is_u_conn_to_src = True break if not is_u_conn_to_src: component_of_v = nx.node_connected_component(g_temp_removed_edge, line_u) # component_of_v is actually comp of u else: return [], 0, 0 else: # Equidistant or complex, cannot determine simple downstream for this line return [], 0, 0 downstream_nodes = component_of_v total_downstream_load_kw = sum(G.nodes[n]['load_kw'] for n in downstream_nodes) total_downstream_dg_kw = sum(G.nodes[n]['dg_kw'] for n in downstream_nodes if G.nodes[n]['dg_kw'] > 0) return list(downstream_nodes), total_downstream_load_kw, total_downstream_dg_kw def calculate_power_flows_and_currents(self, current_graph_state, active_dgs_kw): """ Simplified power flow for radial networks or parts of networks. Returns dict of line flows and currents, and substation powers. Flows: {(u,v): power_kw} where power_kw > 0 means u to v. Currents: {(u,v): current_A} Substation_powers: {'CB1': power_kw_drawn_from_substation} """ line_flows_kw = {} line_currents_a = {} substation_powers_kw = {} # Update DG outputs in the graph state for node_id, dg_val in active_dgs_kw.items(): if node_id in current_graph_state: current_graph_state.nodes[node_id]['dg_kw'] = dg_val for node_id in current_graph_state.nodes(): # Reset others if not in active_dgs_kw if node_id not in active_dgs_kw and 'dg_kw' in current_graph_state.nodes[node_id]: if current_graph_state.nodes[node_id].get('type') != 'source': # Don't zero out if it was never a DG current_graph_state.nodes[node_id]['dg_kw'] = 0 # Determine connected components and their sources # This is a very simplified load flow. It assumes power flows from sources (substations) # down to loads. DG power reduces the load seen by upstream sections. # It does not handle loops well without iterative methods (e.g. Hardy Cross or Newton-Raphson). # For each feeder, calculate flows assuming radial structure # This is an approximation. A full AC or DC power flow is more accurate. # Initialize all line flows to 0 for u, v in current_graph_state.edges(): line_flows_kw[(u,v)] = 0 line_flows_kw[(v,u)] = 0 # For bi-directional calculation needs line_currents_a[(u,v)] = 0 # Iterate multiple times for flow distribution in case of ties or complex paths # This is a placeholder for a proper iterative flow solution. # For now, a topological sort based flow for radial parts. processed_nodes_for_flow_calc = set() for cb_id, (src_node, start_node) in self.substation_connections.items(): if not current_graph_state.has_node(start_node) or not nx.is_connected(current_graph_state.subgraph([n for n in current_graph_state.nodes() if n != SOURCE_NODE])): # This feeder might be entirely down if start_node is disconnected from actual nodes substation_powers_kw[cb_id] = 0 continue # Get nodes for this feeder (dynamic based on current_graph_state) # Nodes connected to start_node, excluding SOURCE_NODE, if start_node is connected to SOURCE_NODE feeder_nodes_component = set() if current_graph_state.has_edge(src_node, start_node): temp_g_for_feeder = current_graph_state.copy() # Remove other substation links to isolate this feeder's component other_links_to_remove = [] for other_cb, (s,d) in self.substation_connections.items(): if other_cb != cb_id and temp_g_for_feeder.has_edge(s,d): other_links_to_remove.append((s,d)) temp_g_for_feeder.remove_edges_from(other_links_to_remove) if nx.has_path(temp_g_for_feeder, src_node, start_node): try: # Consider only the part of the graph reachable from start_node, not crossing back to SOURCE_NODE # except via the designated start_node path. search_nodes = [n for n in temp_g_for_feeder.nodes if n != src_node] sub_graph_feeder = temp_g_for_feeder.subgraph(search_nodes) if start_node in sub_graph_feeder: feeder_nodes_component = nx.node_connected_component(sub_graph_feeder, start_node) except nx.NetworkXError: # if start_node not in subgraph feeder_nodes_component = set() if not feeder_nodes_component: substation_powers_kw[cb_id] = 0 continue # Order nodes from furthest to closest to substation for power accumulation # This is for radial feeders. If loops exist, this is not sufficient. # Using BFS layers from start_node # Net load at each node (Load - DG) node_net_power_kw = {} for node in feeder_nodes_component: node_net_power_kw[node] = current_graph_state.nodes[node]['load_kw'] - current_graph_state.nodes[node]['dg_kw'] # Accumulate power up towards the substation # This requires a tree traversal (e.g., DFS post-order traversal) from leaves to root. # For simplicity, if the feeder is a tree rooted at start_node: if nx.is_tree(current_graph_state.subgraph(feeder_nodes_component | {start_node})): # Check if it's a tree # Create a directed tree towards the source for easier traversal # This part is complex if graph is not a tree. # For now, sum all net loads on the feeder as the substation power (approximation) total_feeder_net_load = sum(node_net_power_kw[n] for n in feeder_nodes_component) substation_powers_kw[cb_id] = total_feeder_net_load # Distribute this flow down the lines (highly simplified) # A proper method: for each line, sum net_power of all nodes in subtree rooted by that line. # This simplified flow calculation is a major placeholder. # For overload risk, we need per-line flows. # Simplified: assume current_graph_state is a tree rooted at start_node for this feeder # Use BFS to assign flow from start_node downwards # This is not a full power flow, but an estimation for line loading. # Build a directed graph for this feeder based on BFS from start_node # This is just for flow assignment direction. # Actual flow needs to sum up demands from downstream. # For each edge (u,v) in the feeder: # Determine parent (closer to start_node) and child # Power on (parent, child) = sum of net loads in subtree rooted at child. # This is complex to implement robustly here without a full flow algorithm. # Fallback: Use the _get_downstream_info logic if possible, iterate edges # This needs to be called carefully to avoid double counting or misdirection in non-radial. # For now, this function will primarily return substation_powers_kw and # leave detailed line_flows_kw and line_currents_a for a more robust implementation # or accept its high level of approximation. # Let's try a slightly better approximation for line flows on a tree: # For each edge (u,v) in the feeder tree (rooted at start_node) # Assume u is parent of v. Power(u,v) = sum of net_power for all nodes in subtree of v. if start_node in feeder_nodes_component: # Should be try: dfs_edges = list(nx.dfs_edges(nx.bfs_tree(current_graph_state.subgraph(feeder_nodes_component), start_node), source=start_node)) # Calculate power for each node including its children's power node_total_subtree_power = node_net_power_kw.copy() for u, v in reversed(dfs_edges): # From leaves up to root if u in node_total_subtree_power and v in node_total_subtree_power: node_total_subtree_power[u] += node_total_subtree_power[v] for u,v in dfs_edges: # From root down if v in node_total_subtree_power: flow = node_total_subtree_power[v] line_flows_kw[(u,v)] = flow line_flows_kw[(v,u)] = -flow # Convention for direction line_currents_a[(u,v)] = self._calculate_line_current_kw(flow) except Exception as e: # print(f"Warning: Could not perform tree-based flow for {cb_id} due to {e}") pass # Keep substation power as sum, line flows might be inaccurate else: # Not a tree, flow calculation is more complex. # print(f"Warning: Feeder {cb_id} is not a tree. Simplified flow may be inaccurate.") total_feeder_net_load = sum(node_net_power_kw[n] for n in feeder_nodes_component if n in node_net_power_kw) substation_powers_kw[cb_id] = total_feeder_net_load # Line flows in meshed networks require iterative solvers. # For now, this part will be very approximate for meshed sections. # Handle reverse power flow and inter-feeder DG adjustment # "分布式能源不得向上级电网倒送功率" # "可以在相邻馈线间进行调节" for cb_id in list(substation_powers_kw.keys()): if substation_powers_kw[cb_id] < 0: # Reverse power flow excess_dg_on_feeder = -substation_powers_kw[cb_id] # Try to transfer to other feeders via inter-feeder tie lines # This logic is complex and needs careful state management. # For Q1, a simpler approach might be DG curtailment on that feeder. # Find inter-feeder tie switches connected to this feeder # Example: S62-3 connects Feeder 3 (node 62) to Feeder 1 (node 19) # If Feeder 1 has excess_dg, it might try to send to Feeder 3 via (19,62) # This is an advanced feature. For now, assume DG curtailment if倒送. # To implement curtailment: identify DGs on this feeder, reduce their output # proportionally until substation_powers_kw[cb_id] >= 0. # This would require re-calculating flows. # For now, just flag it. # print(f"Warning: Reverse power flow on {cb_id} of {substation_powers_kw[cb_id]} kW. DG curtailment or transfer needed.") # A simple curtailment: dG_on_feeder_nodes = [n for n in self.feeder_info.get(cb_id, set()) if n in active_dgs_kw and active_dgs_kw[n] > 0] total_dg_cap_on_feeder = sum(active_dgs_kw[n] for n in dG_on_feeder_nodes) if total_dg_cap_on_feeder > 0: curtail_ratio = min(1.0, excess_dg_on_feeder / total_dg_cap_on_feeder) if total_dg_cap_on_feeder >0 else 0 for dg_node in dG_on_feeder_nodes: active_dgs_kw[dg_node] *= (1 - curtail_ratio) # Flows need to be recalculated after curtailment. This suggests an iterative solution. # For this submission, we'll assume this check is done *before* final flow calc, # or simply note the violation. # To avoid recursion here, this function should ideally take DGs as fixed input. # The adjustment logic should be outside or iterative. pass return line_flows_kw, line_currents_a, substation_powers_kw def calculate_overload_risk(self): """ Calculates overload risk for the current DG setup. Assumes DGs are at their BASE_DG_CAPACITY_KW. """ # Get current operational graph (no faults, ties normally open) g_op = self._get_subgraph_with_operational_lines(self.graph) # Calculate power flows and currents # Need to handle DG outputs properly. current_dg_outputs = self.dg_kw.copy() # Use the model's current DG settings # Iterative step for DG curtailment if reverse power flow: # This is a simplified loop. A more robust solution uses optimization or better heuristics. for _iter in range(3): # Max 3 iterations for adjustment line_flows, line_currents, substation_powers = self.calculate_power_flows_and_currents(g_op, current_dg_outputs) reverse_power_detected = False for cb_id, power_kw in substation_powers.items(): if power_kw < -1e-3: # Small threshold for倒送 reverse_power_detected = True # print(f"Info: Reverse power on {cb_id} ({power_kw:.2f} kW). Attempting curtailment.") excess_dg_on_feeder = abs(power_kw) feeder_nodes_for_cb = self.feeder_info.get(cb_id, set()) dG_on_feeder_nodes = [n for n in feeder_nodes_for_cb if n in current_dg_outputs and current_dg_outputs[n] > 0] total_dg_cap_on_feeder = sum(current_dg_outputs[n] for n in dG_on_feeder_nodes) if total_dg_cap_on_feeder > 1e-3 : # Avoid division by zero curtail_amount_total = excess_dg_on_feeder for dg_node in dG_on_feeder_nodes: # Proportional curtailment proportion = current_dg_outputs[dg_node] / total_dg_cap_on_feeder curtail_this_dg = proportion * curtail_amount_total current_dg_outputs[dg_node] = max(0, current_dg_outputs[dg_node] - curtail_this_dg) else: # No DG to curtail, reverse power might be from other sources or model issue pass if not reverse_power_detected: break # Final flows after potential curtailment line_flows, line_currents, substation_powers = self.calculate_power_flows_and_currents(g_op, current_dg_outputs) overloaded_lines = [] for u, v, data in g_op.edges(data=True): if data.get('type') == 'substation_link': continue # Don't check substation links themselves for overload here current = line_currents.get((u,v), 0) # If flow is from v to u, current might be stored as current_uv = -current_vu # Take absolute value of flow for current calculation, or ensure current is always positive. # The _calculate_line_current_kw uses abs(power_kw) so current should be positive. rated_current = data.get('rated_current_a', DEFAULT_LINE_RATED_CURRENT_A) if current > 1.1 * rated_current: overloaded_lines.append({'edge': (u,v), 'current': current, 'rated': rated_current, 'over_by_%': (current/(1.1*rated_current)-1)*100 if rated_current>0 else float('inf')}) if overloaded_lines: # print(f"System Overload Detected. Overloaded lines: {overloaded_lines}") # P_over = 1 (for this deterministic scenario) # C_over = fixed penalty or sum of penalties risk_overload = 1.0 * COST_PENALTY_FOR_ANY_OVERLOAD # Or, sum of consequences for each overloaded line, if C_over is per line. # risk_overload = sum(some_cost_function(ol['over_by_%']) for ol in overloaded_lines) else: # print("System is NOT overloaded in the base case.") risk_overload = 0.0 return risk_overload, overloaded_lines, substation_powers, current_dg_outputs def calculate_load_loss_risk(self): """ Calculates total load loss risk by considering single line faults. R_loss = sum(P_fault_i * C_loss_i) P_fault_i = annual probability of fault i C_loss_i = consequence of fault i (e.g., unserved_load_kw * COST_VOLL_PER_KW) """ total_load_loss_risk = 0.0 detailed_fault_impacts = [] # Iterate through all operational lines (excluding substation virtual links for fault simulation) original_edges = [ (u,v,data) for u,v,data in self.graph.edges(data=True) if data.get('type') != 'substation_link' and not data.get('is_tie', False)] for u_fault, v_fault, line_data_faulted in original_edges: faulty_edge = (u_fault, v_fault) line_length_km = line_data_faulted.get('length_km', 0) # Probability of this specific line failing (annual) # Assuming failure rates are independent and this is the probability of this line being the one to fail. prob_line_fault = line_length_km * FAILURE_RATE_LINE_PER_KM if prob_line_fault == 0: continue # --- Simulate fault --- g_faulted = self.graph.copy() if not g_faulted.has_edge(*faulty_edge): continue g_faulted.edges[faulty_edge]['failed'] = True # Mark as failed g_after_fault_isolation = self._get_subgraph_with_operational_lines(g_faulted, faulty_edge=faulty_edge) # --- Identify initial load loss --- initial_unserved_load_kw = 0 disconnected_load_nodes = {} # {node: load_kw} # Check connectivity for all load nodes for node_id, load_kw in self.loads_kw.items(): if load_kw <= 0: continue is_connected_to_source = False for cb_id, (src_node, start_node) in self.substation_connections.items(): if nx.has_path(g_after_fault_isolation, src_node, node_id): is_connected_to_source = True break if not is_connected_to_source: initial_unserved_load_kw += load_kw disconnected_load_nodes[node_id] = load_kw if initial_unserved_load_kw == 0: # Fault does not cause load loss (e.g. redundant line) detailed_fault_impacts.append({'fault': faulty_edge, 'unserved_kw_initial': 0, 'unserved_kw_final':0, 'restored_kw':0, 'risk_contrib':0}) continue # --- Attempt restoration via tie lines --- # This is a complex part. Needs to: # 1. Identify disconnected areas and loads. # 2. Identify available tie switches that can connect these areas to healthy feeders. # 3. Check capacity of tie lines and the supporting feeder. # 4. Prioritize restoration (e.g., maximize load restored). # For this model, a simplified restoration: # Iterate over available tie switches. If closing one helps, simulate it. # This should be greedy or more optimized. restored_load_kw_total_for_this_fault = 0 # Create a graph state for restoration attempts g_for_restoration = g_after_fault_isolation.copy() # Sort disconnected loads by size (optional, for prioritization) sorted_disconnected_loads = sorted(disconnected_load_nodes.items(), key=lambda item: item[1], reverse=True) # Try closing tie switches one by one (if they connect a live part to a dead part) # This is a very simplified greedy approach. # A proper approach would evaluate all combinations or use optimization. # Identify current live sources/feeders live_feeder_sources = [] # (cb_id, start_node_of_live_feeder) for cb_id, (src,start) in self.substation_connections.items(): if nx.has_path(g_for_restoration, src, start): # Check if substation itself is connected live_feeder_sources.append(start) for tie in self.tie_switches_info: tie_n1, tie_n2 = tie['nodes'] tie_capacity_a = tie['capacity_A'] if not g_for_restoration.has_node(tie_n1) or not g_for_restoration.has_node(tie_n2): continue if g_for_restoration.has_edge(tie_n1, tie_n2): continue # Already closed or part of main graph (should not be for ties) # Check if one end is live and other is dead (or part of the disconnected component) tie_n1_is_live = any(nx.has_path(g_for_restoration, src, tie_n1) for src in live_feeder_sources) tie_n2_is_live = any(nx.has_path(g_for_restoration, src, tie_n2) for src in live_feeder_sources) if tie_n1_is_live == tie_n2_is_live: continue # Both live or both dead, closing doesn't bridge outage for now live_tie_node, dead_tie_node = (tie_n1, tie_n2) if tie_n1_is_live else (tie_n2, tie_n1) # Check if dead_tie_node is part of the current outage we are trying to fix # This requires knowing which component dead_tie_node belongs to. # For now, assume if it's not live, it's part of some outage. # Simulate closing this tie switch g_for_restoration.add_edge(live_tie_node, dead_tie_node, id=tie['id'], type='tie_closed', rated_current_a=tie_capacity_a, resistance_ohm=0.001, length_km=0.01) # Check how much load can be restored through this tie without overloading tie or new path # This requires a flow calculation on g_for_restoration. # Simplified: Check loads now connected. newly_restored_load_kw_this_tie = 0 temp_restored_nodes_this_tie = [] for node_id, load_val in disconnected_load_nodes.items(): if node_id not in g_for_restoration: continue # Should not happen # Check if this node is now connected to ANY source is_now_connected = any(nx.has_path(g_for_restoration, src, node_id) for src in live_feeder_sources) if is_now_connected and node_id not in temp_restored_nodes_this_tie: # And not already counted as restored by previous ties # More checks needed: # 1. Tie line capacity: Power through (live_tie_node, dead_tie_node) <= tie_capacity_a # 2. Path capacity on the live feeder. # This is where the simplified flow becomes a bottleneck. # For now, assume if connected, it can be restored up to a certain limit. # This is a MAJOR simplification. newly_restored_load_kw_this_tie += load_val temp_restored_nodes_this_tie.append(node_id) # Here, we'd need to check if adding newly_restored_load_kw_this_tie overloads the tie or feeder. # If current_through_tie > tie_capacity_a, then not all of this load can be restored. # This part needs a proper constrained flow allocation. # For now, let's assume a fraction can be restored if connected, or all if small. # This is a placeholder for a more robust restoration algorithm. # Let's assume, for now, if connected, it's restored. If this overloads things, # the overload risk model should capture it (but that's for normal state). # Here, the goal is to minimize unserved load. # If this tie leads to overload, we shouldn't use it or only partially. # For now, naively accept all newly connected load. if newly_restored_load_kw_this_tie > 0: restored_load_kw_total_for_this_fault += newly_restored_load_kw_this_tie # Update disconnected_load_nodes: for r_node in temp_restored_nodes_this_tie: if r_node in disconnected_load_nodes: del disconnected_load_nodes[r_node] # No longer disconnected else: # Closing this tie didn't help, revert if g_for_restoration.has_edge(live_tie_node, dead_tie_node): g_for_restoration.remove_edge(live_tie_node, dead_tie_node) # Final unserved load for this fault scenario final_unserved_load_kw = initial_unserved_load_kw - restored_load_kw_total_for_this_fault final_unserved_load_kw = max(0, final_unserved_load_kw) # Cannot be negative consequence_c_loss = final_unserved_load_kw * COST_VOLL_PER_KW risk_contribution = prob_line_fault * consequence_c_loss total_load_loss_risk += risk_contribution detailed_fault_impacts.append({ 'fault_type': 'line', 'component_id': faulty_edge, 'prob_fault': prob_line_fault, 'unserved_kw_initial': initial_unserved_load_kw, 'restored_kw': restored_load_kw_total_for_this_fault, 'unserved_kw_final': final_unserved_load_kw, 'consequence_c_loss': consequence_c_loss, 'risk_contribution': risk_contribution }) # TODO: Add DG faults, Switch faults, User faults if they cause wider outages. # For DG faults: prob_dg_fault = FAILURE_RATE_DG_PERCENT # A DG fault primarily impacts system's ability to meet load or avoid overload. # It doesn't directly cause load loss unless it's islanded and the DG is the only source. # The problem implies grid-connected DGs. return total_load_loss_risk, detailed_fault_impacts # --- Main Execution --- if __name__ == '__main__': print("--- 配电网风险评估模型 Q1 ---") # 1. Load Data print("\n1. 加载数据...") loads = load_load_data() topology = load_topology_data() # print(f"负荷数据: {len(loads)} 点") # print(f"拓扑数据: {len(topology)} 条线路") # 2. Initialize Power Grid Model print("\n2. 初始化电网模型...") grid = PowerGridModel(loads, topology, DG_LOCATIONS_KW, TIE_SWITCHES_INFO, SUBSTATION_CONNECTIONS) # print(f"电网图: {grid.graph.number_of_nodes()} 个节点, {grid.graph.number_of_edges()} 条边") # print(f"馈线信息: {grid.feeder_info}") # --- 问题1: 失负荷风险和过负荷风险计算模型 --- print("\n--- 问题1: 风险计算 ---") # A. 过负荷风险模型 (R_over = P_over * C_over) # For Q1, DGs are at BASE_DG_CAPACITY_KW. This is a deterministic check for this state. # P_over = 1 if overload occurs, 0 otherwise. C_over is the penalty. print("\nA. 计算过负荷风险...") # Note: The calculate_power_flows_and_currents is highly simplified. # Results for overload depend heavily on its accuracy and line ratings. try: risk_overload, overloaded_lines_details, substation_p, final_dg_out = grid.calculate_overload_risk() print(f" 计算得到的过负荷风险 (R_over): ${risk_overload:.2f}") if overloaded_lines_details: print(f" 检测到过负荷线路 ({len(overloaded_lines_details)} 条):") # for ol in overloaded_lines_details[:3]: # Print first 3 # print(f" - 线路 {ol['edge']}, 电流: {ol['current']:.2f}A, 额定: {ol['rated']:.2f}A, 超出: {ol['over_by_%']:.2f}%") else: print(" 在当前DG配置下，未检测到线路过负荷。") # print(f" 变电站出口功率 (kW): {substation_p}") # print(f" 最终DG出力 (kW) (可能经过削减): {final_dg_out}") except Exception as e: print(f" 计算过负荷风险时发生错误: {e}") risk_overload = -1 # Indicate error # B. 失负荷风险模型 (R_loss = sum(P_fault_i * C_loss_i)) print("\nB. 计算失负荷风险...") # Note: Restoration logic is simplified. try: total_r_loss, fault_details = grid.calculate_load_loss_risk() print(f" 计算得到的总失负荷风险 (R_loss): ${total_r_loss:.2f} (基于所选成本)") # print("\n 部分故障场景详情:") # for fd in fault_details[:3]: # Print first 3 # print(f" - 故障线路: {fd.get('component_id')}, " # f"初始失负荷: {fd.get('unserved_kw_initial'):.2f} kW, " # f"最终失负荷: {fd.get('unserved_kw_final'):.2f} kW, " # f"风险贡献: ${fd.get('risk_contribution'):.2f}") except Exception as e: print(f" 计算失负荷风险时发生错误: {e}") total_r_loss = -1 # Indicate error print("\n--- 模型执行完毕 ---") print("注意: 此模型包含多项简化和假设 (如线路额定电流, 成本参数, 潮流计算简化, 恢复逻辑简化).") print("结果的准确性取决于这些假设的合理性和参数的精确性。") 此代码用到了哪些数学模型，如整数规划、贪心算法等等，把具体用到的模型写出来，并把模型建立的过程写的尽可能完整
05-12
for cb_id, (src_node, start_node) in self.substation_connections.items(): if nx.is_tree(...): dfs_edges = list(nx.dfs_edges(...)) # 后序遍历计算功率 for u, v in reversed(dfs_edges): node_total_...
#include<iostream> #include<cmath> #include <stdexcept> #include <iostream> #include <vector> #include <cmath> #include <ros/ros.h> #include <sensor_msgs/PointCloud2.h> #include <pcl_conversions/pcl_conversions.h> #include "std_msgs/Float64.h" #include "geometry_msgs/Point.h" #include "sensor_msgs/Imu.h" #include "Spline.h" #include "ins/ASENSING.h" #include <std_msgs/Bool.h> #include <visualization_msgs/Marker.h> #include "jsk_recognition_msgs/BoundingBoxArray.h" #define D_ROAD_W 0.08 //道路宽度采样间隔 [m] using namespace std; int load_go=0; // bool start_go=true;//跑包注释 bool start_go; bool load_yaw=false; bool load_location=false; bool correct_data_get=false; bool point_cloud_get = false; double imu_speed; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>); double x_array[26];///改点注释1111111111111111111111111111111 double y_array[26];///改点注释2222222222222222222222 class Point { public: double x ; // m double y ; // m }; class Vehicle_State { public: double x; // m double y; // m double yaw; // degree }; class Vehicle_Trajectory { public: vector<double> x; vector<double> y; double cf; }; int last_tem_index =9; ros::Publisher pub; ros::Publisher pub_stop; /**************/ ros::Publisher marker_pub; ros::Publisher marker_all; ros::Publisher marker_all2; ros::Publisher pub_brake; //2023.8.11 /****************/ std_msgs::Float64 stop_sign; std_msgs::Float64 break_flag; //2023.8.11 Vehicle_State state; double correct_angle; double correct_x; double correct_y; double angleDegrees = 0; Point center1;//第一次更新后的左圆中心 Point center2;//第一次更新后的右圆中心 vector<Point> state_all; vector<Point> obstruct; vector<Point> obstruct_left; vector<Point> obstruct_right; vector<Point> center_left; vector<Point> center_right; vector<Point> obstruct_left_nei; vector<Point> obstruct_left_wai; vector<Point> obstruct_right_nei; vector<Point> obstruct_right_wai; vector<Point> obstruct_left_nei_new; vector<Point> obstruct_left_wai_new; vector<Point> obstruct_right_nei_new; vector<Point> obstruct_right_wai_new; vector<Point> path_global_location; Vehicle_Trajectory tra_best; int location_index = 0; int load = 0; Point center_left_final; Point center_right_final; vector<Point> left_center_sum; vector<Point> right_center_sum; double way_x[26];//改点注释141414141414141414 double way_y[26]; double distance(double x1,double y1,double x2,double y2) { return sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)); } Point findCircleCenter(const std::vector<Point>& points, double radius) { int n = points.size(); // Create matrices for the linear system Eigen::MatrixXd A(n, 3); Eigen::VectorXd b(n); for (int i = 0; i < n; ++i) { A(i, 0) = 2 * (points[i].x - points[0].x); A(i, 1) = 2 * (points[i].y - points[0].y); A(i, 2) = -1; b(i) = pow(points[i].x - points[0].x, 2) + pow(points[i].y - points[0].y, 2) - pow(radius, 2); } // Solve the least squares problem A * [cx, cy, d] = b Eigen::VectorXd solution = A.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(b); Point center; // Extract circle center coordinates center.x = solution(0) + points[0].x; center.y = solution(1) + points[0].y; return center; } void fitCircle(const std::vector<Point>& points, double& centerX, double& centerY, double& radius) { int n = points.size(); if (n < 3) { std::cerr << "At least 3 points are required to fit a circle." << std::endl; return; } double sumX = 0, sumY = 0, sumX2 = 0, sumY2 = 0, sumXY = 0; double sumX3 = 0, sumY3 = 0, sumX1Y2 = 0, sumX2Y1 = 0; for (const auto& p : points) { double x = p.x; double y = p.y; double x2 = x * x; double y2 = y * y; sumX += x; sumY += y; sumX2 += x2; sumY2 += y2; sumXY += x * y; sumX3 += x2 * x; sumY3 += y2 * y; sumX1Y2 += x * y2; sumX2Y1 += x2 * y; } double C = n * sumX2 - sumX * sumX; double D = n * sumXY - sumX * sumY; double E = n * sumX3 + n * sumX1Y2 - (sumX2 + sumY2) * sumX; double G = n * sumY2 - sumY * sumY; double H = n * sumY3 + n * sumX2Y1 - (sumX2 + sumY2) * sumY; double denominator = 2 * (C * G - D * D); if (denominator == 0) { std::cerr << "Denominator in circle fitting is zero." << std::endl; return; } centerX = (G * E - D * H) / denominator; centerY = (C * H - D * E) / denominator; radius = std::sqrt((sumX2 + sumY2 - 2 * centerX * sumX - 2 * centerY * sumY + n * (centerX * centerX + centerY * centerY)) / n); } void interpolatePoints(double* original_x, double* original_y, double* new_x, double* new_y,int SIZE ,int INTERVALS) { int index = 0; for (int i = 0; i < SIZE - 1; ++i) { double x1 = original_x[i]; double y1 = original_y[i]; double x2 = original_x[i + 1]; double y2 = original_y[i + 1]; double dx = (x2 - x1) / INTERVALS; double dy = (y2 - y1) / INTERVALS; for (int j = 0; j < INTERVALS; ++j) { new_x[index] = x1 + j * dx; new_y[index] = y1 + j * dy; index++; } } // Add the last point from original arrays new_x[index] = original_x[SIZE - 1]; new_y[index] = original_y[SIZE - 1]; } void set_path_global_location() { //起点长12 yuanshi double relative_x_chushi[190] = {1.5,3.5, 4.50000000000000, 6, 7.50000000000000, 9,\ 10.5000000000000, 12, 13.5000000000000, 15, 16.4929784907258, 17.9458293799335, 19.3198327455343,\ 20.5757278190614, 21.6828360254628, 22.6094864377429, 23.3294552745182, 23.8273423696585, 24.0851743056398,\ 24.0979134140882, 23.8680591804246, 23.3954329981049, 22.6979300314371, 21.7946808320437, 20.7048142903275,\ 19.4630603273459, 18.1011332458409, 16.6546373468543, 15.1641312227858, 13.6690456433255, 12.2101604570310,\ 10.8256011255841, 9.55468897394665, 8.43058413047132, 7.48147650023618, 6.73850387747837, 6.21667689392197,\ 5.92972783702215, 5.89151242648978, 6.09614442454558, 6.54058093591135, 7.21546395009521, 8.09787282472529,\ 9.16744952227656, 10.3947886499797, 11.7450087033329, 13.1840940648742, 14.6717990386110, 15, 16.4929784907258,\ 17.9458293799335, 19.3198327455343, 20.5757278190614, 21.6828360254628, 22.6094864377429, 23.3294552745182,\ 23.8273423696585, 24.0851743056398, 24.0979134140882, 23.8680591804246, 23.3954329981049, 22.6979300314371,\ 21.7946808320437, 20.7048142903275, 19.4630603273459, 18.1011332458409, 16.6546373468543, 15.1641312227858,\ 13.6690456433255, 12.2101604570310, 10.8256011255841, 9.55468897394665, 8.43058413047132, 7.48147650023618,\ 6.73850387747837, 6.21667689392197, 5.92972783702215, 5.89151242648978, 6.09614442454558, 6.54058093591135,\ 7.21546395009521, 8.09787282472529, 9.16744952227656, 10.3947886499797, 11.7450087033329, 13.1840940648742,\ 14.6717990386110, 15, 16.4929784907258, 17.9458293799335, 19.3198327455343, 20.5757278190614, 21.6828360254628,\ 22.6094864377429, 23.3294552745182, 23.8273423696585, 24.0851743056398, 24.0979134140882, 23.8680591804246,\ 23.3954329981049, 22.6979300314371, 21.7946808320437, 20.7048142903275, 19.4630603273459, 18.1011332458409,\ 16.6546373468543, 15.1641312227858, 13.6690456433255, 12.2101604570310, 10.8256011255841, 9.55468897394665,\ 8.43058413047132, 7.48147650023618, 6.73850387747837, 6.21667689392197, 5.92972783702215, 5.89151242648978,\ 6.09614442454558, 6.54058093591135, 7.21546395009521, 8.09787282472529, 9.16744952227656, 10.3947886499797,\ 11.7450087033329, 13.1840940648742, 14.6717990386110, 15, 16.4929784907258, 17.9458293799335, 19.3198327455343,\ 20.5757278190614, 21.6828360254628, 22.6094864377429, 23.3294552745182, 23.8273423696585, 24.0851743056398,\ 24.0979134140882, 23.8680591804246, 23.3954329981049, 22.6979300314371, 21.7946808320437, 20.7048142903275,\ 19.4630603273459, 18.1011332458409, 16.6546373468543, 15.1641312227858, 13.6690456433255, 12.2101604570310,\ 10.8256011255841, 9.55468897394665, 8.43058413047132, 7.48147650023618, 6.73850387747837, 6.21667689392197,\ 5.92972783702215, 5.89151242648978, 6.09614442454558, 6.54058093591135, 7.21546395009521, 8.09787282472529,\ 9.16744952227656, 10.3947886499797, 11.7450087033329, 13.1840940648742, 14.6717990386110, 15, 16.5000000000000,\ 19, 22, 25, 26, 27, 27.5, 28, 30, 32, 34,36,37,38,39,40,41,42,43,44}; double relative_y_chushi[190] = {0,0, 0, 0, 0, 0, 0, 0, 0, 0, -0.125512998485473, -0.490926224144828,\ -1.08816183366577, -1.90503550632258, -2.91432374260530, -4.09153170739197, -5.40532980676421, -6.81832568457704,\ -8.29414167929603, -9.79219795955772, -11.2726581312459, -12.6943584686520, -14.0201743080130, -15.2155116851562,\ -16.2435583500563, -17.0816257944470, -17.7058883021754, -18.0959745771550, -18.2468458811889, -18.1508600590876,\ -17.8102001737293, -17.2379453204251, -16.4446558358059, -15.4543479571398, -14.2951097755576, -12.9940963464869,\ -11.5898196518438, -10.1193407357934, -8.62161504175242, -7.13755102147194, -5.70681560192370, -4.36927451073298,\ -3.15859392661961, -2.10954692453109, -1.25047108785953, -0.601302290877508, -0.184672791857001, -0.00847771368893407,\ 0, -0.125512998485473, -0.490926224144828, -1.08816183366577, -1.90503550632258, -2.91432374260530, -4.09153170739197,\ -5.40532980676421, -6.81832568457704, -8.29414167929603, -9.79219795955772, -11.2726581312459, -12.6943584686520,\ -14.0201743080130, -15.2155116851562, -16.2435583500563, -17.0816257944470, -17.7058883021754, -18.0959745771550,\ -18.2468458811889, -18.1508600590876, -17.8102001737293, -17.2379453204251, -16.4446558358059, -15.4543479571398,\ -14.2951097755576, -12.9940963464869, -11.5898196518438, -10.1193407357934, -8.62161504175242, -7.13755102147194,\ -5.70681560192370, -4.36927451073298, -3.15859392661961, -2.10954692453109, -1.25047108785953, -0.601302290877508,\ -0.184672791857001, -0.00847771368893407, 0, 0.125512998485473, 0.490926224144828, 1.08816183366577, 1.90503550632258,\ 2.91432374260530, 4.09153170739197, 5.40532980676421, 6.81832568457704, 8.29414167929603, 9.79219795955772, 11.2726581312459,\ 12.6943584686520, 14.0201743080130, 15.2155116851562, 16.2435583500563, 17.0816257944470, 17.7058883021754, 18.0959745771550,\ 18.2468458811889, 18.1508600590876, 17.8102001737293, 17.2379453204251, 16.4446558358059, 15.4543479571398, 14.2951097755576,\ 12.9940963464869, 11.5898196518438, 10.1193407357934, 8.62161504175242, 7.13755102147194, 5.70681560192370, 4.36927451073298,\ 3.15859392661961, 2.10954692453109, 1.25047108785953, 0.601302290877508, 0.184672791857001, 0.00847771368893407, 0,\ 0.125512998485473, 0.490926224144828, 1.08816183366577, 1.90503550632258, 2.91432374260530, 4.09153170739197, 5.40532980676421,\ 6.81832568457704, 8.29414167929603, 9.79219795955772, 11.2726581312459, 12.6943584686520, 14.0201743080130, 15.2155116851562,\ 16.2435583500563, 17.0816257944470, 17.7058883021754, 18.0959745771550, 18.2468458811889, 18.1508600590876, 17.8102001737293,\ 17.2379453204251, 16.4446558358059, 15.4543479571398, 14.2951097755576, 12.9940963464869, 11.5898196518438, 10.1193407357934,\ 8.62161504175242, 7.13755102147194, 5.70681560192370, 4.36927451073298, 3.15859392661961, 2.10954692453109, 1.25047108785953,\ 0.601302290877508, 0.184672791857001, 0.00847771368893407, 0, 0, 0,0,0,0,0,0,0,0,0,0,0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}; double relative_x[950]; double relative_y[950]; interpolatePoints(relative_x_chushi, relative_y_chushi, relative_x, relative_y,190,5); for(int i=0;i<950;i++) //12米 { relative_x[i]=relative_x[i]+ correct_x; relative_y[i]=relative_y[i]+ correct_y; } correct_angle = angleDegrees; double point_center_x = 15.0; double point_center_y = 0.0; //预设点集的八字中心点坐标 double cosAngle = cos(-correct_angle* M_PI/180.0); double sinAngle = sin(-correct_angle* M_PI/180.0); for(int i=0;i<950;i++) //12米 { double temp_x = relative_x[i]; double temp_y = relative_y[i]; relative_x[i] = cosAngle * (temp_x - point_center_x) - sinAngle * (temp_y - point_center_y) + point_center_x; relative_y[i] = sinAngle * (temp_x - point_center_x) + cosAngle * (temp_y - point_center_y) + point_center_y; Point temp; temp.x = relative_x[i]; temp.y = relative_y[i]; state_all.push_back(temp); } Point trans_point; for(int i=0;i<950;i++)//转换为世界坐标 改长注释22222222222222222222222222222 { trans_point.x = state.x + (relative_x[i]+2.38)*cos(state.yaw*M_PI/180) + relative_y[i]*sin(state.yaw*M_PI/180); trans_point.y = state.y - relative_y[i]*cos(state.yaw*M_PI/180) + (relative_x[i]+2.38)*sin(state.yaw*M_PI/180); path_global_location.push_back(trans_point); } } Point trans_local_xy(Point global_xy)//把相对于世界坐标的点转到激光雷达坐标系下 { Point xy_local; double trans_x = (global_xy.x-state.x)*cos(-state.yaw*M_PI/180) - (global_xy.y-state.y)*sin(-state.yaw*M_PI/180) - 2.38; double trans_y = - (global_xy.y-state.y)*cos(-state.yaw*M_PI/180) - (global_xy.x-state.x)*sin(-state.yaw*M_PI/180); xy_local.x = trans_x; xy_local.y = trans_y; return xy_local; } void start_sign(const std_msgs::Float64& msg) { load_go = msg.data; cout<<"load_go= %d!!!"<<load_go<<endl; if(load_go==1) { cout<<"start go!!!"<<endl; start_go=true; }else{ cout<<"wait for go"<<endl; } } void zhuitongCallback(const jsk_recognition_msgs::BoundingBoxArray::ConstPtr& msg) { obstruct.clear(); obstruct_left.clear(); obstruct_right.clear(); obstruct_left_nei.clear(); obstruct_left_wai.clear(); obstruct_right_nei.clear(); obstruct_right_wai.clear(); obstruct_left_nei_new.clear(); obstruct_left_wai_new.clear(); obstruct_right_nei_new.clear(); obstruct_right_wai_new.clear(); Point temp; for(int i = 0;i<msg->boxes.size();i++) { temp.x= msg->boxes[i].pose.position.x; temp.y= msg->boxes[i].pose.position.y; float dis = distance(0,0,temp.x,temp.y); if(dis<1.5||dis>29) { continue; } obstruct.push_back(temp); if(temp.y>0) { obstruct_left.push_back(temp); }else if(temp.y<0) { obstruct_right.push_back(temp); } } if(correct_data_get == false) { if(obstruct.size()>0) { point_cloud_get = true; } if(obstruct_left.size()>2) { Point test; test = findCircleCenter(obstruct_left,9.125); for(auto &obs_temp : obstruct_left) { double dis_temp = distance(obs_temp.x,obs_temp.y,test.x,test.y); if(dis_temp>=9.125) { obstruct_left_wai.push_back(obs_temp); }else { obstruct_left_nei.push_back(obs_temp); } } } // cout<<"x_left:"<<test.x<<endl; // cout<<"y_left:"<<test.y<<endl; if(obstruct_right.size()>2) { Point test2; test2 = findCircleCenter(obstruct_right,9.125); for(auto &obs_temp2 : obstruct_right) { double dis_temp2 = distance(obs_temp2.x,obs_temp2.y,test2.x,test2.y); if(dis_temp2>=9.125) { obstruct_right_wai.push_back(obs_temp2); }else { obstruct_right_nei.push_back(obs_temp2); } } } // cout<<"x_right:"<<test2.x<<endl; // cout<<"y_right:"<<test2.y<<endl; if(obstruct_left_nei.size()>2) { temp = findCircleCenter(obstruct_left_nei,7.625); left_center_sum.push_back(temp); } if(obstruct_left_wai.size()>2) { temp = findCircleCenter(obstruct_left_wai,10.625); left_center_sum.push_back(temp); } if(obstruct_right_nei.size()>2) { temp = findCircleCenter(obstruct_right_nei,7.625); right_center_sum.push_back(temp); } if(obstruct_right_wai.size()>2) { temp = findCircleCenter(obstruct_right_wai,10.625); right_center_sum.push_back(temp); } // cout<<"left:"<<left_center_sum.size()<<endl; // cout<<"right:"<<right_center_sum.size()<<endl; if(right_center_sum.size()>20&&left_center_sum.size()>20) { double left_x_sum = 0; double left_y_sum = 0; for(int i = 0;i<left_center_sum.size();i++) { left_x_sum +=left_center_sum[i].x; left_y_sum +=left_center_sum[i].y; } center1.x = left_x_sum/(left_center_sum.size()); center1.y = left_y_sum/(left_center_sum.size()); double right_x_sum = 0; double right_y_sum = 0; for(int i = 0;i<right_center_sum.size();i++) { right_x_sum +=right_center_sum[i].x; right_y_sum +=right_center_sum[i].y; } center2.x = right_x_sum/(right_center_sum.size()); center2.y = right_y_sum/(right_center_sum.size()); } if(center1.x!=0&&center1.y!=0&&center2.x!=0&&center2.y!=0&&obstruct_left.size()>2&&obstruct_right.size()>2) { for(auto &obs_temp : obstruct_left) { double dis_temp = distance(obs_temp.x,obs_temp.y,center1.x,center1.y); if(dis_temp>=9.125) { obstruct_left_wai_new.push_back(obs_temp); }else { obstruct_left_nei_new.push_back(obs_temp); } } for(auto &obs_temp2 : obstruct_right) { double dis_temp2 = distance(obs_temp2.x,obs_temp2.y,center2.x,center2.y); if(dis_temp2>=9.125) { obstruct_right_wai_new.push_back(obs_temp2); }else { obstruct_right_nei_new.push_back(obs_temp2); } } Point center; if(obstruct_left_nei_new.size()>2) { center = findCircleCenter(obstruct_left_nei_new,7.625); center_left.push_back(center); } if(obstruct_left_wai_new.size()>2) { center = findCircleCenter(obstruct_left_wai_new,10.625); center_left.push_back(center); } if(obstruct_right_nei_new.size()>2) { center = findCircleCenter(obstruct_right_nei_new,7.625); center_right.push_back(center); } if(obstruct_right_wai_new.size()>2) { center = findCircleCenter(obstruct_right_wai_new,10.625); center_right.push_back(center); } Point left_sum; Point right_sum; if(center_left.size()>20&&center_right.size()>20) { for(int i = 0;i<center_left.size();i++) { left_sum.x += center_left[i].x; left_sum.y += center_left[i].y; right_sum.x += center_right[i].x; right_sum.y += center_right[i].y; } center_left_final.x = left_sum.x / center_left.size(); center_left_final.y = left_sum.y / center_left.size(); center_right_final.x = right_sum.x / center_right.size(); center_right_final.y = right_sum.y / center_right.size(); } if(center_left_final.x!=0&&center_left_final.y!=0&&center_right_final.x!=0&&center_right_final.y!=0) { center_left_final.x = center_left_final.x ; center_left_final.y = center_left_final.y ; center_right_final.x = center_right_final.x ; center_right_final.y = center_right_final.y ; correct_x = (center_left_final.x + center_right_final.x)/2 - 15; correct_y = (center_left_final.y + center_right_final.y)/2 - 0; double angle = atan2(center_right_final.x - center_left_final.x, center_right_final.y - center_left_final.y); angleDegrees = angle * 180.0 / M_PI-180; if(angleDegrees<-180) { angleDegrees = angleDegrees+360.0; } correct_data_get = true; // cout<<"x:"<<correct_x+15<<endl; // cout<<"y:"<<correct_y<<endl; } } } // visualization_msgs::Marker line_strip_all; // line_strip_all.header.frame_id = "hesai40"; // line_strip_all.header.stamp = ros::Time::now(); // line_strip_all.ns = "trajectory_all"; // line_strip_all.action = visualization_msgs::Marker::ADD; // line_strip_all.pose.orientation.w = 0.0; // line_strip_all.id = 0; // line_strip_all.type = visualization_msgs::Marker::POINTS; // // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width // line_strip_all.scale.x = 0.5; // // Line strip 是绿色 // line_strip_all.color.g = 1.0f; // line_strip_all.color.a = 1.0; // for (int i = 0; i < obstruct_left_nei_new.size(); ++i) // { // geometry_msgs::Point p2; // p2.x = obstruct_left_nei_new[i].x; // p2.y = obstruct_left_nei_new[i].y;//(int32_t) // p2.z = 0; // line_strip_all.points.push_back(p2); // } // //发布各个markers // marker_all.publish(line_strip_all); visualization_msgs::Marker line_strip_all2; line_strip_all2.header.frame_id = "hesai40"; line_strip_all2.header.stamp = ros::Time::now(); line_strip_all2.ns = "trajectory_all2"; line_strip_all2.action = visualization_msgs::Marker::ADD; line_strip_all2.pose.orientation.w = 0.0; line_strip_all2.id = 0; line_strip_all2.type = visualization_msgs::Marker::POINTS; // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width line_strip_all2.scale.x = 0.5; // Line strip 是绿色 line_strip_all2.color.r = 1.0f; line_strip_all2.color.g = 0; line_strip_all2.color.a = 1.0; geometry_msgs::Point p3; p3.x = center_left_final.x; p3.y = center_left_final.y;//(int32_t) p3.z = 0; line_strip_all2.points.push_back(p3); p3.x = center_right_final.x; p3.y = center_right_final.y;//(int32_t) p3.z = 0; line_strip_all2.points.push_back(p3); //发布各个markers marker_all2.publish(line_strip_all2); } void update_yaw_location(const ins::ASENSING::ConstPtr& msg) { state.yaw = msg->azimuth; load_yaw = true; state.x = msg->location_x; state.y = msg->location_y; load_location = true; imu_speed=msg->absolute_velocity; } vector<Vehicle_Trajectory> calc_frenet_paths(double x_array[],double y_array[],int array_size) { vector<Vehicle_Trajectory> frenet_paths; vector<double> x_a,y_a; vector<double> angle; vector<Point> sp; for(int i = 0;i<array_size;++i) { double at_angle; if(x_array[i] == 0) { x_array[i] = 0.000001; } at_angle = -atan(y_array[i]/x_array[i]); angle.push_back(at_angle); } for(int i=-9;i<=+9;++i) { Vehicle_Trajectory fp; vector<double> s_x; vector<double> s_y; s_x.push_back(0.0); s_y.push_back(0.0); for(int j = 0;j<array_size;++j) { Point sp_temp; sp_temp.x = x_array[j]+i*D_ROAD_W*sin(angle[j]); sp_temp.y = y_array[j]+i*D_ROAD_W*cos(angle[j]); s_x.push_back(sp_temp.x); s_y.push_back(sp_temp.y); } for(int i=0;i<10;++i) { double h_x = (s_x[i+1]-s_x[i])/9; double h_y = (s_y[i+1]-s_y[i])/9; for(int j=0;j<10;j++) { fp.x.push_back(h_x*j+s_x[i]); fp.y.push_back(h_y*j+s_y[i]); x_a.push_back(h_x*j+s_x[i]); y_a.push_back(h_y*j+s_y[i]); } } frenet_paths.push_back(fp); } return frenet_paths; } Vehicle_Trajectory calc_cost(Vehicle_Trajectory tr_p,vector<Point> ob)///计算路径点中离锥桶的最小距离 { tr_p.cf = 100; for(int i=0;i<ob.size();i++) { for(int j=0;j<tr_p.x.size();j++) { double dist = distance(ob[i].x, ob[i].y,tr_p.x[j], tr_p.y[j]); if(dist<tr_p.cf) { tr_p.cf = dist; } } } return tr_p; } vector<Vehicle_Trajectory> check_paths(vector<Vehicle_Trajectory> fplist, vector<Point> ob) { vector<Vehicle_Trajectory> fpl; Vehicle_Trajectory calc; cout<<"ob.size()="<<ob.size()<<endl; for(int i=0;i<fplist.size();i++) { calc = calc_cost(fplist[i],ob);// 碰撞检查 //计算每条路径点中离锥桶的最小距离 fpl.push_back(calc); } return fpl; } Vehicle_Trajectory frenet_optimal_planning_nopoint(double x_array[],double y_array[],int array_size) { vector<Vehicle_Trajectory> fplist; fplist = calc_frenet_paths(x_array,y_array,array_size); Vehicle_Trajectory bestpath = fplist[last_tem_index]; return bestpath; } Vehicle_Trajectory frenet_optimal_planning(vector<Point> ob,double x_array[],double y_array[],int &last_tem_index,int array_size) //最佳路径计算函数 { vector<Vehicle_Trajectory> fplist; fplist = calc_frenet_paths(x_array,y_array,array_size); //cout<<"fplist.size()="<<fplist.size()<<endl; vector<Vehicle_Trajectory> fpl_1; fpl_1 = check_paths(fplist, ob);/////////计算每条路径与锥桶的最小距离 // cout<<"fpl_1.size()="<<fpl_1.size()<<endl; int tem_index =last_tem_index; int size=fpl_1.size(); if(location_index<10) { size=fpl_1.size()/2; } int start=0; if(location_index>84&&location_index<100) { start=0; size=fpl_1.size()*1/2; } if(location_index>164&&location_index<180) { start=fpl_1.size()/2; size=fpl_1.size()*2/3; cout<<"!!!!!!!!!!!!!!!!!!!!!!!"<<endl; cout<<"33333333333333333333333"<<endl; } if(location_index>=0&&location_index<10) { for(start;start<size;start++) { if(abs(start-last_tem_index)<3) { if(fpl_1[tem_index].cf < fpl_1[start].cf)//找到与锥桶最小距离最小的路径序号 { tem_index = start; } } } } if(location_index>=10&&location_index<84) { for(start;start<size;start++) { if(abs(start-last_tem_index)<3) { if(fpl_1[tem_index].cf < fpl_1[start].cf)//找到与锥桶最小距离最小的路径序号 { for(start;start<16;start++) { double t=fpl_1[start].cf; if(t>1.0) { tem_index = start; break;} } if(tem_index>13){tem_index=13;} if(tem_index<4){tem_index=4;} } } } } if(location_index>=84&&location_index<90) { for(start;start<size;start++) { if(abs(start-last_tem_index)<4) { if(fpl_1[tem_index].cf < fpl_1[start].cf)//找到与锥桶最小距离最小的路径序号 { double t=fpl_1[start].cf; if(t>1.0) { tem_index = start; if(tem_index>13){tem_index=13;} if(tem_index<4){tem_index=4;} break;} } } } } if(location_index>=90&&location_index<180) { for(start;start<size;start++) { if(abs(start-last_tem_index)<4) { if(fpl_1[tem_index].cf < fpl_1[start].cf)//找到与锥桶最小距离最小的路径序号 { double t=fpl_1[start].cf; if(t>1.0&&t<2.0) { tem_index = start; if(tem_index>13){tem_index=13;} if(tem_index<4){tem_index=4;} break;} } } } } cout<<"tem_index="<<tem_index<<endl; Vehicle_Trajectory bestpath = fpl_1[tem_index]; last_tem_index =tem_index; return bestpath; } int search_index(Vehicle_State point_search,vector<Point> point_all) { Vehicle_State current_point; Vehicle_State point; std::map<double,Vehicle_State>min_distance; for(auto &iter_p : point_all) { double distance2 = std::sqrt((point_search.x - iter_p.x)*(point_search.x - iter_p.x)+(point_search.y - iter_p.y)*(point_search.y - iter_p.y)); current_point.x=iter_p.x; current_point.y=iter_p.y; min_distance.insert(std::make_pair(distance2,current_point)); } auto min = min_distance.begin(); point = min->second; for(int i=0;i<point_all.size();i++) { if(point.x==point_all[i].x&&point.y==point_all[i].y) { if(point.x<point_search.x) { return i+1; } return i; } } return 0; }; void path_plan() { if(load_yaw ==true && load_location==true && correct_data_get==true) { sensor_msgs::PointCloud2 output; pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud1 (new pcl::PointCloud<pcl::PointXYZRGB>); if(load == 0) { set_path_global_location(); load++; location_index = search_index(state,path_global_location); cout<<"location_index:"<<location_index<<endl; if(location_index>50) { location_index = 50; } location_index = 0; } double distance;//车前路径点与车的距离 distance = hypot((path_global_location[location_index].x - state.x),(path_global_location[location_index].y - state.y)); ROS_INFO("distance=%f",distance); if(distance <4)//1111111111111111111111111111111111111111111111111小心!!!!!!!!!!!!!! { for(int i=0;i<26;i++)//车前路径点和顺下点 ///改点注释3333333333333333333333333333333333333 { way_x[i] = path_global_location[location_index+i].x; way_y[i] = path_global_location[location_index+i].y; cout<<"x:"<<way_x[i]<<setprecision(8)<<endl; cout<<"y:"<<way_y[i]<<setprecision(8)<<endl; } location_index++; } break_flag.data=0; //2023.8.11 if(location_index==435)//87 { break_flag.data=35; std::cout<<break_flag.data<<"yyyyyyyyyyyyyyyyyyyyy"<<std::endl; } if(location_index==430)//86 { break_flag.data=45; std::cout<<break_flag.data<<"yyyyyyyyyyyyyyyyyyyyy"<<std::endl; } pub_brake.publish(break_flag); //2023.8.11 for(int i=0;i<26;i++)//将路径点转换为激光雷达坐标下的点///改点注释444444444444444444444444444444 { Point tr_xy; Point lo_xy; tr_xy.x = way_x[i]; tr_xy.y = way_y[i]; lo_xy = trans_local_xy(tr_xy); x_array[i] = lo_xy.x; y_array[i] = lo_xy.y; } int array_size = sizeof(x_array)/sizeof(x_array[0]); if(point_cloud_get==true) { tra_best = frenet_optimal_planning(obstruct,x_array,y_array,last_tem_index,array_size); point_cloud_get = false; } else { tra_best = frenet_optimal_planning_nopoint(x_array,y_array,array_size); } if(location_index >= 850) { stop_sign.data = 1; cout<<" stop_sign.data = "<<stop_sign.data<<endl; ROS_INFO("Vehicle has reached the target point! Node stop!"); pub_stop.publish(stop_sign); } (*cloud1).width=tra_best.x.size();//zuiyou lujing //无序点云width为点云大小，heigh为１ (*cloud1).height=1; (*cloud1).is_dense=false; //默认为false (*cloud1).points.resize(tra_best.x.size()); //点云大小 for(int i=0;i<tra_best.x.size();i++) { (*cloud1)[i].x=tra_best.x[i]; (*cloud1)[i].y=tra_best.y[i]; // cout<<"(*cloud1)[i].x="<<(*cloud1)[i].x<<endl; // cout<<"(*cloud1)[i].y="<<(*cloud1)[i].y<<endl; (*cloud1)[i].z=0; (*cloud1)[i].r=0; (*cloud1)[i].g=255; (*cloud1)[i].b=0; } pcl::toROSMsg(*cloud1,output); //第一个参数是输入，后面的是输出 output.header.frame_id = "hesai40";//输出的坐标 pub.publish(output);///修改注释11111111111111111111111111111111111111111111111111111 visualization_msgs::Marker line_strip; line_strip.header.frame_id = "hesai40"; line_strip.header.stamp = ros::Time::now(); line_strip.ns = "trajectory"; line_strip.action = visualization_msgs::Marker::ADD; line_strip.pose.orientation.w = 0.0; line_strip.id = 0; line_strip.type = visualization_msgs::Marker::LINE_STRIP; // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width line_strip.scale.x = 0.1; // Line strip 是绿色 line_strip.color.g = 1.0f; line_strip.color.a = 1.0; // cout<<"size:"<<tra_best.x.size()<<endl; for (int i = 0; i < tra_best.x.size(); ++i) { geometry_msgs::Point p; p.x = tra_best.x[i]; p.y = tra_best.y[i];//(int32_t) p.z = 0; line_strip.points.push_back(p); } //发布各个markers marker_pub.publish(line_strip); visualization_msgs::Marker line_strip_all; line_strip_all.header.frame_id = "hesai40"; line_strip_all.header.stamp = ros::Time::now(); line_strip_all.ns = "trajectory_all"; line_strip_all.action = visualization_msgs::Marker::ADD; line_strip_all.pose.orientation.w = 0.0; line_strip_all.id = 0; line_strip_all.type = visualization_msgs::Marker::LINE_STRIP; // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width line_strip_all.scale.x = 0.1; // Line strip 是绿色 line_strip_all.color.g = 1.0f; line_strip_all.color.a = 1.0; for (int i = 0; i < path_global_location.size(); ++i) { geometry_msgs::Point p2; Point global_point; global_point = trans_local_xy(path_global_location[i]); p2.x = global_point.x; p2.y = global_point.y;//(int32_t) p2.z = 0; line_strip_all.points.push_back(p2); } //发布各个markers marker_all.publish(line_strip_all); // visualization_msgs::Marker line_strip_all; // line_strip_all.header.frame_id = "hesai40"; // line_strip_all.header.stamp = ros::Time::now(); // line_strip_all.ns = "trajectory_all"; // line_strip_all.action = visualization_msgs::Marker::ADD; // line_strip_all.pose.orientation.w = 0.0; // line_strip_all.id = 0; // line_strip_all.type = visualization_msgs::Marker::POINTS; // // LINE_STRIP/LINE_LIST markers use only the x component of scale, for the line width // line_strip_all.scale.x = 0.5; // // Line strip 是绿色 // line_strip_all.color.g = 1.0f; // line_strip_all.color.a = 1.0; // for (int i = 0; i < obstruct_left_nei.size(); ++i) // { // geometry_msgs::Point p2; // p2.x = obstruct_left_nei[i].x; // p2.y = obstruct_left_nei[i].y;//(int32_t) // p2.z = 0; // line_strip_all.points.push_back(p2); // } // //发布各个markers // marker_all.publish(line_strip_all); } } int main(int argc,char** argv) { ros::init(argc,argv,"trajectory"); ros::NodeHandle n; stop_sign.data = 0; ros::Rate r(100); ros::Subscriber sub_ecu = n.subscribe ("/ECU_Data", 1, start_sign); ros::Subscriber sub_yaw_location = n.subscribe ("/INS/ASENSING_INS", 1, update_yaw_location); ros::Subscriber pub_bbox = n.subscribe<jsk_recognition_msgs::BoundingBoxArray>("/detections", 10, zhuitongCallback); marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 1); marker_all = n.advertise<visualization_msgs::Marker>("visualization_all", 1); marker_all2 = n.advertise<visualization_msgs::Marker>("visualization_all2", 1); pub = n.advertise<sensor_msgs::PointCloud2> ("/trajectory", 1);//定义一个发布者，发布最后的轨迹////修改注释222222222222 pub_stop = n.advertise<std_msgs::Float64>("/stop_sign",1); pub_brake = n.advertise<std_msgs::Float64>("/brake",1); while (ros::ok()) { path_plan(); ros::spinOnce(); // Keeps node alive basically r.sleep(); // Sleep for loop_rate } } 上述代码是一个八字绕环路径规划的代码 路径是什么样的呢？你能画出来吗
10-07
for (float t = 2*M_PI; t >= 0; t -= step) { x = r + r * cos(t); y = 0 + r * sin(t); 将点(x,y)加入路径点列表。 } 注意：第二个圆的旋转方向与第一个圆相反（顺时针），所以角度从2π递减到0。这样第二个...
CreateBitmap
07-15 1464
函数功能：该函数创建一个带有特定宽度、高度和颜色格式的位图。    函数原型：HBITMAP CreateBitmap(int nWidth,int nHeight, UINT cPlanes, UINT cBitsPeral,CONST VOID *lpvBits)；    参
enum-props:枚举，支持其他元数据
05-12
介绍 enum-props包装了出色的enum实现 。 它公开了一个Enum工厂，该工厂允许将其他元数据添加到每个枚举成员。 安装 npm install -S enum-props 用法 var Enum = require ( 'enum-props' ) ; // define a simple enum (automatically flaggable -> A: 0x01, B: 0x02, C: 0x04) //Uses bitwise 'OR' operation in between the values and creates enumerated constants. For example, if 'Read':1, 'Write':2, then ReadWrite= Read | Write = 1 | 2 = 3; var myEnum = new Enum (
map
h_11qx的博客
11-28 295
选择某种Map集合保存学号从1到15的学员的学号（键）和姓名（值），学号用字符串表示，输入的时候要以学号乱序的方式存入Map集合，然后按照学号从大到小的顺序将Map集合中的元素输出打印。需要自定义Map集合的比较器Comparator，因字符串对象的大小比较是按字典序，而非对应的数值。 要求：必须使用Map集合的内部排序机制进行排序，不能在外部排序。 import java.util.
STL源码剖析 容器 stl_map.h
07-22 1354
map -------------------------------------------------------------------------------- 所有元素都会根据元素的键值自动被排序。 map的所有元素都是 pair，同时拥有实值和键值。 不可以修改元素的键值，因为它关系到 map 元素的排列规则 可以修改元素的实值，因为它不影响 map 的排列规则 map iterator 既不是一种 constant iterators ， 也不是一种 mutable iterator 标准
create_dib_bitmap.cpp
CCodes的专栏
11-08 512
 
《STL源码剖析》---stl_map.h阅读笔记
KangRoger的专栏
08-16 5393
map是STL中的标准关系容器，它存储的元素时pair，拥有键值key和实值value。按照键值key存储到红黑树中，用红黑树提供的函数操作map。
enumprops.cpp
CCodes的专栏
11-08 678
  #include   #include "EnumProps.h" #if defined (WIN32) #define IS_WIN32 TRUE#else #define IS_WIN32 FALSE#endif#define IS_NT      IS_WIN32 && (BOOL)(GetVersion() #define IS_WIN32S  IS_WIN32 && (BO
CollectionView中deleteItems方法的使用
weixin_30301449的博客
10-31 790
最近在做一个批量删除照片的功能,调用了 deleteItems这个方法,但是使用这个方法之后程序崩溃,报错:You need to also delete associated data from the data source as well.这是因为在使用这个方法时,要删除数据源数据,我在项目中利用冒泡将indexPath进行有小到大排序,重新进行遍历,将其存到数组中,这样就可以实现照片的选中...
CreateDibBitmap为什么总是失败？
haohamaru的专栏
04-14 1091
 函数原型：HBITMAP CreateDIBitmap(HDC hdc,CONST BITMAPINFONEADER *lpbmih,DWORD fdwlnit,CONST VOID *lpblnit,CONST BITMAPINFO *lpbmi,UINT fuUsage)；    参数：    hdc：设备环境句柄。    lpbmih：指向位图信息头结构的指针，它可以是下列操作
文本查询TextQuery类文件编写
Michael是个半路程序员
10-19 1508
读取用户指定的任意文本文件，然后允许用户从该文件中查找单词。查询的结果是该单词出现的次数，并列出每次出现所在的行。如果某单词在同一行中多次出现，程序将只显示该行一次。行号按升序显示，即第 7 行应该在第 9 行之前输出，依此类推。 textquery.h 头文件定义文件 #ifndef TEXTQUERY_H //防止重复定义，名称大写 #define TEXTQUERY_H #include&...
评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
红包 添加红包
表情包 插入表情
表情包 代码片
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值