行为规划伪代码

实现转换函数的一种方法是为每个可访问的“下一个状态”生成粗略轨迹，然后找到最佳状态。为了“找到最好的”，我们通常使用成本函数。

然后我们可以计算出每条粗略轨迹的代价是多大，然后选择成本轨迹最低的状态。

稍后我们会更详细地讨论这一点，但首先仔细阅读下面的伪代码，以更好地了解转换函数的工作原理。

def transition_function(predictions, current_fsm_state, current_pose, cost_functions, weights):    # only consider states which can be reached from current FSM state.    possible_successor_states = successor_states(current_fsm_state)    # keep track of the total cost of each state.    costs = []    for state in possible_successor_states:        # generate a rough idea of what trajectory we would        # follow IF we chose this state.        trajectory_for_state = generate_trajectory(state, current_pose, predictions)        # calculate the "cost" associated with that trajectory.        cost_for_state = 0        for i in range(len(cost_functions)) :            # apply each cost function to the generated trajectory            cost_function = cost_functions[i]            cost_for_cost_function = cost_function(trajectory_for_state, predictions)            # multiply the cost by the associated weight            weight = weights[i]            cost_for_state += weight * cost_for_cost_function         costs.append({
    'state' : state, 'cost' : cost_for_state})    # Find the minimum cost state.    best_next_state = None    min_cost = 9999999    for i in range(len(possible_successor_states)):        state = possible_successor_states[i]        cost  = costs[i]        if cost < min_cost:            min_cost = cost            best_next_state = state     return best_next_state

显然，我们在这里重申了一些重要的细节。即：什么是这些成本函数以及我们如何建立呢？接下来我们将讨论这个问题！