add decition/testapollompc for test mpc controller.

src/decition/testapollompc/Readme.MD

@@ -0,0 +1,4 @@
+1.apollo代码从内部git下载。相应pb.cc文件用protoc生成。
+2.osqp从moduliazation_thirdpartylib下载。
+3.absl同上。
+

src/decition/testapollompc/main.cpp

@@ -0,0 +1,131 @@
+#include <QCoreApplication>
+
+#include "modules/control/controller/mpc_controller.h"
+
+#include "cyber/common/file.h"
+#include "cyber/common/log.h"
+#include "gmock/gmock.h"
+#include "gtest/gtest.h"
+
+#include "cyber/time/clock.h"
+#include "modules/common/vehicle_state/vehicle_state_provider.h"
+#include "modules/control/common/control_gflags.h"
+#include "modules/control/proto/control_conf.pb.h"
+#include "modules/localization/common/localization_gflags.h"
+#include "modules/planning/proto/planning.pb.h"
+
+namespace apollo {
+namespace control {
+
+using apollo::cyber::Clock;
+using PlanningTrajectoryPb = planning::ADCTrajectory;
+using LocalizationPb = localization::LocalizationEstimate;
+using ChassisPb = canbus::Chassis;
+using apollo::common::VehicleStateProvider;
+
+class MPCControllerTest : public ::testing::Test, MPCController {
+ public:
+  virtual void SetupTestCase() {
+    FLAGS_v = 4;
+    FLAGS_control_conf_file =
+        "/apollo/modules//control/testdata/mpc_controller_test/"
+        "control_conf.pb.txt ";
+    ControlConf control_conf;
+    ACHECK(cyber::common::GetProtoFromFile(FLAGS_control_conf_file,
+                                           &control_conf));
+    mpc_conf_ = control_conf.mpc_controller_conf();
+
+    timestamp_ = Clock::NowInSeconds();
+  }
+
+  void ComputeLateralErrors(const double x, const double y, const double theta,
+                            const double linear_v, const double angular_v,
+                            const double linear_a,
+                            const TrajectoryAnalyzer &trajectory_analyzer,
+                            SimpleMPCDebug *debug) {
+    MPCController::ComputeLateralErrors(x, y, theta, linear_v, angular_v,
+                                        linear_a, trajectory_analyzer, debug);
+  }
+
+ protected:
+  LocalizationPb LoadLocalizaionPb(const std::string &filename) {
+    LocalizationPb localization_pb;
+    ACHECK(cyber::common::GetProtoFromFile(filename, &localization_pb))
+        << "Failed to open file " << filename;
+    localization_pb.mutable_header()->set_timestamp_sec(timestamp_);
+    return localization_pb;
+  }
+
+  ChassisPb LoadChassisPb(const std::string &filename) {
+    ChassisPb chassis_pb;
+    ACHECK(cyber::common::GetProtoFromFile(filename, &chassis_pb))
+        << "Failed to open file " << filename;
+    chassis_pb.mutable_header()->set_timestamp_sec(timestamp_);
+    return chassis_pb;
+  }
+
+  PlanningTrajectoryPb LoadPlanningTrajectoryPb(const std::string &filename) {
+    PlanningTrajectoryPb planning_trajectory_pb;
+    ACHECK(cyber::common::GetProtoFromFile(filename, &planning_trajectory_pb))
+        << "Failed to open file " << filename;
+    planning_trajectory_pb.mutable_header()->set_timestamp_sec(timestamp_);
+    return planning_trajectory_pb;
+  }
+
+  MPCControllerConf mpc_conf_;
+
+  double timestamp_ = 0.0;
+};
+
+TEST_F(MPCControllerTest, ComputeLateralErrors) {
+  auto localization_pb = LoadLocalizaionPb(
+      "/home/yuchuli/File/git/apollo/modules//control/testdata/mpc_controller_test/"
+      "1_localization.pb.txt");
+  auto chassis_pb = LoadChassisPb(
+      "/home/yuchuli/File/git/apollo/modules/control/testdata/mpc_controller_test/1_chassis.pb.txt");
+  FLAGS_enable_map_reference_unify = false;
+  auto injector = std::make_shared<DependencyInjector>();
+  auto vehicle_state = injector->vehicle_state();
+  vehicle_state->Update(localization_pb, chassis_pb);
+
+  auto planning_trajectory_pb = LoadPlanningTrajectoryPb(
+      "/home/yuchuli/File/git/apollo/modules//control/testdata/mpc_controller_test/"
+      "1_planning.pb.txt");
+  TrajectoryAnalyzer trajectory_analyzer(&planning_trajectory_pb);
+
+  ControlCommand cmd;
+  SimpleMPCDebug *debug = cmd.mutable_debug()->mutable_simple_mpc_debug();
+
+  ComputeLateralErrors(
+      vehicle_state->x(), vehicle_state->y(), vehicle_state->heading(),
+      vehicle_state->linear_velocity(), vehicle_state->angular_velocity(),
+      vehicle_state->linear_acceleration(), trajectory_analyzer, debug);
+
+  double theta_error_expected = -0.03549;
+  double theta_error_dot_expected = 0.0044552856731;
+  double d_error_expected = 1.30917375441;
+  double d_error_dot_expected = 0.0;
+  double matched_theta_expected = -1.81266;
+  double matched_kappa_expected = -0.00237307;
+
+  EXPECT_NEAR(debug->heading_error(), theta_error_expected, 0.001);
+  EXPECT_NEAR(debug->heading_error_rate(), theta_error_dot_expected, 0.001);
+  EXPECT_NEAR(debug->lateral_error(), d_error_expected, 0.001);
+  EXPECT_NEAR(debug->lateral_error_rate(), d_error_dot_expected, 0.001);
+  EXPECT_NEAR(debug->ref_heading(), matched_theta_expected, 0.001);
+  EXPECT_NEAR(debug->curvature(), matched_kappa_expected, 0.001);
+}
+
+}  // namespace control
+}  // namespace apollo
+
+int main(int argc, char *argv[])
+{
+    QCoreApplication a(argc, argv);
+
+    testing::InitGoogleTest(&argc,argv);
+
+    RUN_ALL_TESTS();
+
+    return a.exec();
+}

src/decition/testapollompc/testapollompc.pro

@@ -0,0 +1,141 @@
+QT -= gui
+
+CONFIG += c++11 console
+CONFIG -= app_bundle
+
+# The following define makes your compiler emit warnings if you use
+# any Qt feature that has been marked deprecated (the exact warnings
+# depend on your compiler). Please consult the documentation of the
+# deprecated API in order to know how to port your code away from it.
+DEFINES += QT_DEPRECATED_WARNINGS
+
+# You can also make your code fail to compile if it uses deprecated APIs.
+# In order to do so, uncomment the following line.
+# You can also select to disable deprecated APIs only up to a certain version of Qt.
+#DEFINES += QT_DISABLE_DEPRECATED_BEFORE=0x060000    # disables all the APIs deprecated before Qt 6.0.0
+
+SOURCES += \
+        ../../../thirdpartylib/apollo/cyber/binary.cc \
+        ../../../thirdpartylib/apollo/cyber/common/file.cc \
+        ../../../thirdpartylib/apollo/cyber/common/global_data.cc \
+        ../../../thirdpartylib/apollo/cyber/proto/choreography_conf.pb.cc \
+        ../../../thirdpartylib/apollo/cyber/proto/classic_conf.pb.cc \
+        ../../../thirdpartylib/apollo/cyber/proto/cyber_conf.pb.cc \
+        ../../../thirdpartylib/apollo/cyber/proto/perf_conf.pb.cc \
+        ../../../thirdpartylib/apollo/cyber/proto/run_mode_conf.pb.cc \
+        ../../../thirdpartylib/apollo/cyber/proto/scheduler_conf.pb.cc \
+        ../../../thirdpartylib/apollo/cyber/proto/transport_conf.pb.cc \
+        ../../../thirdpartylib/apollo/cyber/time/clock.cc \
+        ../../../thirdpartylib/apollo/cyber/time/duration.cc \
+        ../../../thirdpartylib/apollo/cyber/time/time.cc \
+        ../../../thirdpartylib/apollo/modules/canbus/proto/chassis.pb.cc \
+        ../../../thirdpartylib/apollo/modules/common/configs/config_gflags.cc \
+        ../../../thirdpartylib/apollo/modules/common/configs/proto/vehicle_config.pb.cc \
+        ../../../thirdpartylib/apollo/modules/common/configs/vehicle_config_helper.cc \
+        ../../../thirdpartylib/apollo/modules/common/filters/digital_filter.cc \
+        ../../../thirdpartylib/apollo/modules/common/filters/digital_filter_coefficients.cc \
+        ../../../thirdpartylib/apollo/modules/common/filters/mean_filter.cc \
+        ../../../thirdpartylib/apollo/modules/common/math/aabox2d.cc \
+        ../../../thirdpartylib/apollo/modules/common/math/box2d.cc \
+        ../../../thirdpartylib/apollo/modules/common/math/line_segment2d.cc \
+        ../../../thirdpartylib/apollo/modules/common/math/linear_interpolation.cc \
+        ../../../thirdpartylib/apollo/modules/common/math/math_utils.cc \
+        ../../../thirdpartylib/apollo/modules/common/math/mpc_osqp.cc \
+        ../../../thirdpartylib/apollo/modules/common/math/polygon2d.cc \
+        ../../../thirdpartylib/apollo/modules/common/math/search.cc \
+        ../../../thirdpartylib/apollo/modules/common/math/vec2d.cc \
+        ../../../thirdpartylib/apollo/modules/common/proto/drive_state.pb.cc \
+        ../../../thirdpartylib/apollo/modules/common/proto/error_code.pb.cc \
+        ../../../thirdpartylib/apollo/modules/common/proto/geometry.pb.cc \
+        ../../../thirdpartylib/apollo/modules/common/proto/header.pb.cc \
+        ../../../thirdpartylib/apollo/modules/common/proto/pnc_point.pb.cc \
+        ../../../thirdpartylib/apollo/modules/common/proto/vehicle_signal.pb.cc \
+        ../../../thirdpartylib/apollo/modules/common/vehicle_state/proto/vehicle_state.pb.cc \
+        ../../../thirdpartylib/apollo/modules/common/vehicle_state/vehicle_state_provider.cc \
+        ../../../thirdpartylib/apollo/modules/control/common/control_gflags.cc \
+        ../../../thirdpartylib/apollo/modules/control/common/interpolation_1d.cc \
+        ../../../thirdpartylib/apollo/modules/control/common/interpolation_2d.cc \
+        ../../../thirdpartylib/apollo/modules/control/common/trajectory_analyzer.cc \
+        ../../../thirdpartylib/apollo/modules/control/controller/mpc_controller.cc \
+        ../../../thirdpartylib/apollo/modules/control/proto/calibration_table.pb.cc \
+        ../../../thirdpartylib/apollo/modules/control/proto/control_cmd.pb.cc \
+        ../../../thirdpartylib/apollo/modules/control/proto/control_conf.pb.cc \
+        ../../../thirdpartylib/apollo/modules/control/proto/gain_scheduler_conf.pb.cc \
+        ../../../thirdpartylib/apollo/modules/control/proto/input_debug.pb.cc \
+        ../../../thirdpartylib/apollo/modules/control/proto/lat_controller_conf.pb.cc \
+        ../../../thirdpartylib/apollo/modules/control/proto/leadlag_conf.pb.cc \
+        ../../../thirdpartylib/apollo/modules/control/proto/lon_controller_conf.pb.cc \
+        ../../../thirdpartylib/apollo/modules/control/proto/mpc_controller_conf.pb.cc \
+        ../../../thirdpartylib/apollo/modules/control/proto/mrac_conf.pb.cc \
+        ../../../thirdpartylib/apollo/modules/control/proto/pad_msg.pb.cc \
+        ../../../thirdpartylib/apollo/modules/control/proto/pid_conf.pb.cc \
+        ../../../thirdpartylib/apollo/modules/dreamview/proto/chart.pb.cc \
+        ../../../thirdpartylib/apollo/modules/localization/common/localization_gflags.cc \
+        ../../../thirdpartylib/apollo/modules/localization/proto/localization.pb.cc \
+        ../../../thirdpartylib/apollo/modules/localization/proto/localization_status.pb.cc \
+        ../../../thirdpartylib/apollo/modules/localization/proto/pose.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_clear_area.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_crosswalk.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_geometry.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_id.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_junction.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_lane.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_overlap.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_parking_space.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_pnc_junction.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_road.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_rsu.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_signal.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_speed_bump.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_stop_sign.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/proto/map_yield_sign.pb.cc \
+        ../../../thirdpartylib/apollo/modules/map/relative_map/proto/navigation.pb.cc \
+        ../../../thirdpartylib/apollo/modules/perception/proto/perception_obstacle.pb.cc \
+        ../../../thirdpartylib/apollo/modules/perception/proto/traffic_light_detection.pb.cc \
+        ../../../thirdpartylib/apollo/modules/planning/proto/decision.pb.cc \
+        ../../../thirdpartylib/apollo/modules/planning/proto/math/fem_pos_deviation_smoother_config.pb.cc \
+        ../../../thirdpartylib/apollo/modules/planning/proto/open_space_task_config.pb.cc \
+        ../../../thirdpartylib/apollo/modules/planning/proto/planner_open_space_config.pb.cc \
+        ../../../thirdpartylib/apollo/modules/planning/proto/planning.pb.cc \
+        ../../../thirdpartylib/apollo/modules/planning/proto/planning_config.pb.cc \
+        ../../../thirdpartylib/apollo/modules/planning/proto/planning_internal.pb.cc \
+        ../../../thirdpartylib/apollo/modules/planning/proto/sl_boundary.pb.cc \
+        ../../../thirdpartylib/apollo/modules/planning/proto/task_config.pb.cc \
+        ../../../thirdpartylib/apollo/modules/routing/proto/routing.pb.cc \
+        main.cpp
+
+# Default rules for deployment.
+qnx: target.path = /tmp/$${TARGET}/bin
+else: unix:!android: target.path = /opt/$${TARGET}/bin
+!isEmpty(target.path): INSTALLS += target
+
+HEADERS += \
+    ../../../thirdpartylib/apollo/modules/canbus/proto/chassis.pb.h \
+    ../../../thirdpartylib/apollo/modules/common/configs/proto/vehicle_config.pb.h \
+    ../../../thirdpartylib/apollo/modules/common/proto/drive_state.pb.h \
+    ../../../thirdpartylib/apollo/modules/common/proto/error_code.pb.h \
+    ../../../thirdpartylib/apollo/modules/common/proto/geometry.pb.h \
+    ../../../thirdpartylib/apollo/modules/common/proto/header.pb.h \
+    ../../../thirdpartylib/apollo/modules/common/proto/pnc_point.pb.h \
+    ../../../thirdpartylib/apollo/modules/common/proto/vehicle_signal.pb.h \
+    ../../../thirdpartylib/apollo/modules/control/controller/mpc_controller.h \
+    ../../../thirdpartylib/apollo/modules/map/proto/map_rsu.pb.h \
+    ../../../thirdpartylib/apollo/modules/planning/proto/planning.pb.h
+
+
+INCLUDEPATH += /usr/include/eigen3
+INCLUDEPATH += $$PWD/../../../thirdpartylib/apollo
+
+INCLUDEPATH += ../../..//thirdpartylib/osqp-0.5.0/include
+INCLUDEPATH += ../../../thirdpartylib/abseil-cpp
+
+LIBS += -lprotobuf -lglog -lgflags -lgtest
+
+
+LIBS += -L$$PWD/../../../thirdpartylib/grpc/lib
+LIBS +=  -labsl_raw_hash_set -labsl_hashtablez_sampler -labsl_exponential_biased -labsl_hash -labsl_bad_variant_access -labsl_city -labsl_status -labsl_cord -labsl_str_format_internal -labsl_synchronization -labsl_graphcycles_internal -labsl_symbolize -labsl_demangle_internal -labsl_stacktrace -labsl_debugging_internal -labsl_malloc_internal -labsl_time -labsl_time_zone -labsl_civil_time -labsl_strings -labsl_strings_internal -labsl_throw_delegate -labsl_int128 -labsl_base -labsl_spinlock_wait -labsl_bad_optional_access -labsl_raw_logging_internal -labsl_log_severity
+
+LIBS += -L$$PWD/../../../thirdpartylib/osqp-0.5.0/build/out
+
+LIBS += -losqp