cos_theta_smoother.h(用户可以调用这个函数CosThetaSmoother::Solve()函数优化自己的路径)
#pragma once
#include <utility>
#include <vector>
#include "modules/planning/proto/cos_theta_smoother_config.pb.h"
namespace apollo {
namespace planning {
class CosThetaSmoother {
public:
explicit CosThetaSmoother(const CosThetaSmootherConfig& config);
bool Solve(const std::vector<std::pair<double, double>>& raw_point2d,
const std::vector<double>& bounds, std::vector<double>* opt_x,
std::vector<double>* opt_y);
private:
CosThetaSmootherConfig config_;
};
} // namespace planning
} // namespace apollo
cos_theta_smoother.cc(源文件,为Ipopt::TNLP优化库配置参数和用户的路径点)
#include "modules/planning/math/discretized_points_smoothing/cos_theta_smoother.h"
#include "IpIpoptApplication.hpp"
#include "IpSolveStatistics.hpp"
#include "cyber/common/log.h"
#include "modules/planning/math/discretized_points_smoothing/cos_theta_ipopt_interface.h"
namespace apollo {
namespace planning {
CosThetaSmoother::CosThetaSmoother(const CosThetaSmootherConfig& config)
: config_(config) {}
bool CosThetaSmoother::Solve(
const std::vector<std::pair<double, double>>& raw_point2d,
const std::vector<double>& bounds, std::vector<double>* opt_x,
std::vector<double>* opt_y) {
const double weight_cos_included_angle = config_.weight_cos_included_angle();
const double weight_anchor_points = config_.weight_anchor_points();
const double weight_length = config_.weight_length();
const size_t print_level = config_.print_level();
const size_t max_num_of_iterations = config_.max_num_of_iterations();
const size_t acceptable_num_of_iterations =
config_.acceptable_num_of_iterations();
const double tol = config_.tol();
const double acceptable_tol = config_.acceptable_tol();
const bool use_automatic_differentiation =
config_.ipopt_use_automatic_differentiation();
CosThetaIpoptInterface* smoother =
new CosThetaIpoptInterface(raw_point2d, bounds);
smoother->set_weight_cos_included_angle(weight_cos_included_angle);
smoother->set_weight_anchor_points(weight_anchor_points);
smoother->set_weight_length(weight_length);
smoother->set_automatic_differentiation_flag(use_automatic_differentiation);
Ipopt::SmartPtr<Ipopt::TNLP> problem = smoother;
// Create an instance of the IpoptApplication
Ipopt::SmartPtr<Ipopt::IpoptApplication> app = IpoptApplicationFactory();
app->Options()->SetIntegerValue("print_level", static_cast<int>(print_level));
app->Options()->SetIntegerValue("max_iter",
static_cast<int>(max_num_of_iterations));
app->Options()->SetIntegerValue(
"acceptable_iter", static_cast<int>(acceptable_num_of_iterations));
app->Options()->SetNumericValue("tol", tol);
app->Options()->SetNumericValue("acceptable_tol", acceptable_tol);
Ipopt::ApplicationReturnStatus status = app->Initialize();
if (status != Ipopt::Solve_Succeeded) {
AERROR << "*** Error during initialization!";
return false;
}
status = app->OptimizeTNLP(problem);
if (status == Ipopt::Solve_Succeeded ||
status == Ipopt::Solved_To_Acceptable_Level) {
// Retrieve some statistics about the solve
Ipopt::Index iter_count = app->Statistics()->IterationCount();
ADEBUG << "*** The problem solved in " << iter_count << " iterations!";
} else {
AERROR << "Solver fails with return code: " << static_cast<int>(status);
return false;
}
smoother->get_optimization_results(opt_x, opt_y);
return true;
}
} // namespace planning
} // namespace apollo
cos_theta_smoother_config.proto
配置文件
syntax = "proto2";
package apollo.planning;
message CosThetaSmootherConfig {
optional double weight_cos_included_angle = 1 [default = 10000.0];
optional double weight_anchor_points = 2 [default = 1.0];
optional double weight_length = 3 [default = 1.0];
// ipopt settings
optional int32 print_level = 4 [default = 0];
optional int32 max_num_of_iterations = 5 [default = 500];
optional int32 acceptable_num_of_iterations = 6 [default = 15];
optional double tol = 7 [default = 1e-8];
optional double acceptable_tol = 8 [default = 1e-1];
optional bool ipopt_use_automatic_differentiation = 9 [default = false];
}
cos_theta_smoother.h为用户的接口函数,cos_theta_ipopt_interface.h为Ipopt::TNLP优化方法库的接口函数。
bool Solve(const std::vector<std::pair<double, double>>& raw_point2d,
const std::vector<double>& bounds, std::vector<double>* opt_x,
std::vector<double>* opt_y);