def get_plan()

in robots/LoCoBot/locobot_control/nodes/pointrobot3factor_ros_server.py [0:0]

• 70 lines of code
• 11 McCabe index (conditional complexity)

def get_plan(start_conf_val, start_vel, end_conf_val, end_vel, sdf, params):

    start_conf = start_conf_val
    end_conf = end_conf_val

    avg_vel = (end_conf_val - start_conf_val / params.total_time_step) / params.delta_t

    # plot param

    # init optimization
    graph = NonlinearFactorGraph()
    init_values = Values()

    for i in range(0, params.total_time_step + 1):
        key_pos = symbol(ord("x"), i)
        key_vel = symbol(ord("v"), i)

        #% initialize as straight line in conf space
        pose = start_conf_val
        vel = avg_vel

        init_values.insert(key_pos, pose)
        init_values.insert(key_vel, vel)

        #% start/end priors
        if i == 0:
            graph.push_back(PriorFactorVector(key_pos, start_conf, params.pose_fix))
            graph.push_back(PriorFactorVector(key_vel, start_vel, params.vel_fix))
        elif i == params.total_time_step:
            graph.push_back(PriorFactorVector(key_pos, end_conf, params.pose_fix_goal))
            graph.push_back(PriorFactorVector(key_vel, end_vel, params.vel_fix_goal))

        graph.add(VehicleDynamicsFactorVector(key_pos, key_vel, params.cost_sigma))

        # GP priors and cost factor
        if i > 0:
            # graph.push_back(PriorFactorVector(key_pos, end_conf, params.pose_fix_goal))
            # graph.push_back(PriorFactorVector(key_vel, end_vel, params.vel_fix_goal))
            key_pos1 = symbol(ord("x"), i - 1)
            key_pos2 = symbol(ord("x"), i)
            key_vel1 = symbol(ord("v"), i - 1)
            key_vel2 = symbol(ord("v"), i)

            temp = GaussianProcessPriorLinear(
                key_pos1, key_vel1, key_pos2, key_vel2, params.delta_t, params.Qc_model
            )
            graph.push_back(temp)

            #% cost factor
            graph.push_back(
                ObstaclePlanarSDFFactorPointRobot(
                    key_pos,
                    params.pR_model,
                    sdf,
                    params.cost_sigma,
                    params.epsilon_dist,
                )
            )

            #% GP cost factor
            if params.use_GP_inter and params.check_inter > 0:
                for j in range(1, params.check_inter + 1):
                    tau = j * (params.total_time_sec / params.total_check_step)
                    graph.add(
                        ObstaclePlanarSDFFactorGPPointRobot(
                            key_pos1,
                            key_vel1,
                            key_pos2,
                            key_vel2,
                            params.pR_model,
                            sdf,
                            params.cost_sigma,
                            params.epsilon_dist,
                            params.Qc_model,
                            params.delta_t,
                            tau,
                        )
                    )

    if params.use_trustregion_opt:
        parameters = DoglegParams()
        optimizer = DoglegOptimizer(graph, init_values, parameters)
    else:
        parameters = GaussNewtonParams()
        optimizer = GaussNewtonOptimizer(graph, init_values, parameters)

    print("Initial Error = %d\n", graph.error(init_values))

    optimizer.optimizeSafely()
    result = optimizer.values()

    print("Final Error = %d\n", graph.error(result))

    res_flag = True
    if graph.error(result) > params.acceptable_error_threshold:
        res_flag = False
    return result, res_flag