Apollo5.5规划代码分析（2）_costhetaipoptinterface

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
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23

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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73

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];
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

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);
1
2
3

1.cos_theta_smoother_config.proto配置文件配置的参数都有哪些意义

2.调用这个函数需要输入原始点x，y坐标，但是这个bound的容器里面是的消息类型double具体指什么没有搞清楚，输出的opt_x,opt_y为优化后的离散点，但具体怎么优化不清楚，以及优化的结果。以后会移植到ROS中，以及将结果可视化。