modularization/src/decition/testapollompc/main.cpp

#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"

#include "modules/common/configs/config_gflags.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


//using apollo::cyber::Clock;
//using PlanningTrajectoryPb = planning::ADCTrajectory;
//using LocalizationPb = localization::LocalizationEstimate;
//using ChassisPb = canbus::Chassis;
//using apollo::common::VehicleStateProvider;

double timestamp_ = 0.0;

apollo::localization::LocalizationEstimate LoadLocalizaionPb(const std::string &filename) {
  apollo::localization::LocalizationEstimate localization_pb;
  ACHECK(apollo::cyber::common::GetProtoFromFile(filename, &localization_pb))
      << "Failed to open file " << filename;
  localization_pb.mutable_header()->set_timestamp_sec(timestamp_);
  return localization_pb;
}

apollo::canbus::Chassis LoadChassisPb(const std::string &filename) {
  apollo::canbus::Chassis chassis_pb;
  ACHECK(apollo::cyber::common::GetProtoFromFile(filename, &chassis_pb))
      << "Failed to open file " << filename;
  chassis_pb.mutable_header()->set_timestamp_sec(timestamp_);
  return chassis_pb;
}



apollo::planning::ADCTrajectory LoadPlanningTrajectoryPb(const std::string &filename) {
  apollo::planning::ADCTrajectory planning_trajectory_pb;
  ACHECK(apollo::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;
}

void mpctest()
{

    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;
    FLAGS_vehicle_config_path  = "/home/yuchuli/qt/modularization/thirdpartylib/apollo/modules/common/data/vehicle_param.pb.txt";
    auto injector = std::make_shared<apollo::control::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");
    apollo::control::TrajectoryAnalyzer trajectory_analyzer(&planning_trajectory_pb);

    apollo::control::ControlCommand cmd;

    apollo::control::ControlConf  xConf;

    std::string confilename = "/home/yuchuli/qt/modularization/thirdpartylib/apollo/modules/control/testdata/mpc_controller_test/control_conf.pb.txt";
    ACHECK(apollo::cyber::common::GetProtoFromFile(confilename, &xConf))
        << "Failed to open file " << confilename;

    apollo::control::MPCController xController;
    xController.Init(injector,&xConf);

    xController.ComputeControlCommand(&localization_pb,&chassis_pb,&planning_trajectory_pb,&cmd);

    int a = 1;
    a= 2;



}
int main(int argc, char *argv[])
{
    QCoreApplication a(argc, argv);

    testing::InitGoogleTest(&argc,argv);

    mpctest();

//    RUN_ALL_TESTS();

    return a.exec();
}