Apollo搭建control轻量级仿真闭环

最新推荐文章于 2023-11-04 11:05:15 发布

书痴熊

最新推荐文章于 2023-11-04 11:05:15 发布

阅读量206

点赞数 1

分类专栏： apollo研究文章标签：自动驾驶

本文链接：https://blog.csdn.net/xiaohukuzai/article/details/132523352

版权

apollo研究专栏收录该内容

3 篇文章 3 订阅

订阅专栏

背景
在研究Apollo开源代码时，需要闭环才能更好的理解代码。但个人读者研究，受到资金和精力限制，无法购买硬件设备或无力调试实体小车。而无论是基于强动力学(Carsim, CarMaker等)的仿真或场景仿真(LGsimulator, Carla，Prescan等)都相对麻烦，且对电脑性能要求高。本文旨在通过code实现一简单车辆模型，达到闭环调试Apollo代码的目的。
实现原理

用运动学模型模拟实车，接受control模块的控车信号，产生运动。基于定位的初始位置姿态信息推演仿真车辆实时的位置姿态，可以把该位姿信息以/apollo/localization/pose的通道发出，频率设定为100hz。根据control的挂挡和加速转向指令模拟出仿真车辆的底盘信息，以/apollo/canbus/chassis的通道发出，频率设定为100hz。在仿真里可模拟各种障碍物，以/apollo/prediction的通道发出。障碍物信息读者可自己构建，本文不详细说明。
代码讲解

#include <cstdio>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>

#include "master/modules/control/common/control_gflags.h"
#include "cyber/common/file.h"
#include "cyber/common/log.h"
#include "cyber/cyber.h"
#include "cyber/init.h"
#include "cyber/time/rate.h"
#include "cyber/time/time.h"
#include "gflags/gflags.h"
#include "modules/common/adapters/adapter_gflags.h"
#include "modules/common/math/math_utils.h"
#include "modules/common/math/quaternion.h"
#include "modules/common/time/time.h"

#include "modules/common/pnc_point.pb.h"
#include "modules/localization/proto/localization.pb.h"
#include "modules/control/proto/control_cmd.pb.h"
#include "modules/canbus/proto/chassis.pb.h"

using namespace std;
using apollo::cyber::Reader;
using apollo::cyber::Time;
using apollo::cyber::Writer;
using apollo::canbus::Chassis;
using apollo::common::Point3D;
using apollo::common::Angle3D;
using apollo::common::Quaternion;
using apollo::common::TrajectoryPoint;
using apollo::localization::LocalizationStatus;
using apollo::common::math::HeadingToQuaternion;
using apollo::common::math::InverseQuaternionRotate;
using apollo::common::time::Clock;
using apollo::control::ControlCommand;
using apollo::localization::LocalizationEstimate;
using std::this_thread::sleep_for;

DEFINE_string(fake_initial_localization_file,
              "apollo/modules/control/conf"
              "fake_initial_localization.pb.txt",
              "Used for set inital localization.");

DEFINE_int32(feed_frequency, 100,
             "Frequency with which protos are fed to control.");
DEFINE_double(diff, 0.1, "Length of control_ms timeout.");

static double kInitialXOffset = 0.0;
static double kInitialYOffset = 0.0;
static double kInitialZOffset = 0.0;
static double kInitialHeading = 0.0;

struct SimCarStatus {
  double x;
  double y;
  double z;
  double yaw;
  double v;
  double acceleration;
  double yaw_rate;
  double steer;
  double angular_speed;
  apollo::canbus::Chassis::GearPosition current_gear;
  apollo::canbus::Chassis::DrivingMode current_driving_mode;
};
double current_x = 0.0;
double current_y = 0.0;
double current_z = 0.0;
double current_heading = 0.0;
double current_speed = 0.0;
double simcar_period = 0.01;
double steer_ratio = 20;
double wheelbase = 2.87;
double max_steer_angle = 490;
double acceleration = 0.0;
double steering_target = 0.0;
double steering_rate = 0.0;
apollo::canbus::Chassis::GearPosition gear_location;
std::shared_ptr<Reader<ControlCommand>> control_reader;
std::shared_ptr<Writer<LocalizationEstimate>> localization_writer;
std::shared_ptr<Writer<Chassis>> chassis_writer;

void PublishLocalization(const SimCarStatus* simcar_status);
void PublishCanbus(const SimCarStatus* simcar_status);
void fake_chassis(const double acc, const double steer, const double steer_rate,
                  const apollo::canbus::Chassis::GearPosition gear,
                  SimCarStatus* simcar_status);

void TransformToVRF(const Angle3D& point_mrf, const Quaternion& orientation,
                    Angle3D* point_vrf);

void TransformToVRF(const Angle3D& point_mrf, const Quaternion& orientation,
                    Angle3D* point_vrf) {
  Eigen::Vector3d v_mrf(point_mrf.roll(), point_mrf.pitch(), point_mrf.yaw());
  auto v_vrf = InverseQuaternionRotate(orientation, v_mrf);
  point_vrf->set_roll(v_vrf.x());
  point_vrf->set_pitch(v_vrf.y());
  point_vrf->set_yaw(v_vrf.z());
}

void fake_chassis(const double acc, const double steer, const double steer_rate,
                  const apollo::canbus::Chassis::GearPosition gear,
                  SimCarStatus* simcar_status) {
  double angular_speed_ = (current_speed * tan(steer / 100 * max_steer_angle *
                                               (M_PI / 180) / steer_ratio)) /
                          wheelbase;
  current_speed += acc * simcar_period;
  current_speed = apollo::common::math::Clamp(current_speed, 0.0, 33.34);
  current_heading = current_heading + angular_speed_ * simcar_period;
  current_x = current_x + current_speed *
                              cos(current_heading + kInitialHeading) *
                              simcar_period;
  current_y = current_y + current_speed *
                              sin(current_heading + kInitialHeading) *
                              simcar_period;
  current_z = 0.0;
  simcar_status->x = current_x + kInitialXOffset;
  simcar_status->y = current_y + kInitialYOffset;
  simcar_status->z = current_z + kInitialZOffset;
  simcar_status->yaw = current_heading + kInitialHeading;
  simcar_status->v = current_speed;
  simcar_status->current_gear = gear;
  simcar_status->steer = steer;
  simcar_status->angular_speed = angular_speed_;
  simcar_status->acceleration = acc;
}

void PublishCanbus(const SimCarStatus* simcar_status) {
  Chassis chassis_;
  chassis_.mutable_header()->set_timestamp(Clock::NowInSeconds());
  chassis_.set_speed_mps(simcar_status->v);
  chassis_.set_longitudinal_velocity(simcar_status->v);
  chassis_.set_acc_o(simcar_status->acceleration);
  chassis_.set_steering_percentage(simcar_status->steer);
  chassis_.set_gear_location(simcar_status->current_gear);
  chassis_.set_driving_mode(simcar_status->current_driving_mode);
  chassis_writer->Write(chassis_);
}

void PublishLocalization(const SimCarStatus* simcar_status) {
  LocalizationEstimate localization_;
  localization_.mutable_header()->set_timestamp(Clock::NowInSeconds());
  auto* pose = localization_.mutable_global_localization()->mutable_pose();
  auto* motion = localization_.mutable_global_localization()->mutable_motion();
  localization_.mutable_global_localization()->set_status(LocalizationStatus::OK);
  pose->mutable_position()->set_x(simcar_status->x);
  pose->mutable_position()->set_y(simcar_status->y);
  pose->mutable_position()->set_z(simcar_status->z);
  double cur_heading = simcar_status->yaw;
  double cur_speed = simcar_status->v;
  Eigen::Quaternion<double> cur_orientation =
      HeadingToQuaternion<double>(cur_heading);
  pose->mutable_orientation()->set_qw(cur_orientation.w());
  pose->mutable_orientation()->set_qx(cur_orientation.x());
  pose->mutable_orientation()->set_qy(cur_orientation.y());
  pose->mutable_orientation()->set_qz(cur_orientation.z());
  motion->mutable_linear_vel()->set_x(std::cos(cur_heading) * cur_speed);
  motion->mutable_linear_vel()->set_y(std::sin(cur_heading) * cur_speed);
  motion->mutable_linear_vel()->set_z(0.0);
  motion->mutable_angular_vel()->set_roll(0);
  motion->mutable_angular_vel()->set_pitch(0);
  motion->mutable_angular_vel()->set_yaw(simcar_status->angular_speed);
  pose->mutable_euler_angles()->set_roll(0.0);
  pose->mutable_euler_angles()->set_pitch(0.0);
  pose->mutable_euler_angles()->set_yaw(cur_heading);
  TransformToVRF(motion->angular_vel(), pose->orientation(),
                 motion->mutable_angular_vel_vrf());

  localization_writer->Write(localization_);
}

int main(int argc, char** argv) {
  google::ParseCommandLineFlags(&argc, &argv, true);
  apollo::cyber::Init(argv[0]);
  FLAGS_alsologtostderr = true;

  shared_ptr<apollo::cyber::Node> node(
      apollo::cyber::CreateNode("simulator_node"));
  control_reader =
      node->CreateReader<ControlCommand>(FLAGS_control_command_topic, nullptr);
  localization_writer =
      node->CreateWriter<LocalizationEstimate>(FLAGS_localization_topic);
  chassis_writer = node->CreateWriter<Chassis>(FLAGS_chassis_topic);
  SimCarStatus simcar_status;

  TrajectoryPoint initial_point;

  if (!apollo::cyber::common::GetProtoFromFile(
          FLAGS_fake_initial_localization_file, &initial_point)) {
    AERROR << "failed to load file: " << FLAGS_fake_initial_localization_file;
    return -1;
  } else {
    kInitialXOffset = initial_point.path_point().x();
    kInitialYOffset = initial_point.path_point().y();
    kInitialZOffset = initial_point.path_point().z();
    kInitialHeading = initial_point.path_point().theta();
    current_speed = initial_point.v();
  }

  while (apollo::cyber::OK()) {
    double current_timestamp = Clock::NowInSeconds();
    cout << "current_timestamp = " << setprecision(12) << current_timestamp
         << endl;
    control_reader->Observe();
    const auto& control_msg = control_reader->GetLatestObserved();
    if (control_msg == nullptr) {
      cout << "control_msg is empty!" << endl;
      acceleration = -1.0;
      steering_target = 0.0;
      steering_rate = 100.0;
      gear_location = apollo::canbus::Chassis::GEAR_NEUTRAL;
      simcar_status.current_driving_mode =
          apollo::canbus::Chassis::COMPLETE_MANUAL;
    } else if (current_timestamp - control_msg->header().timestamp() >
               FLAGS_diff) {
      cout << "control_msg is timeout!" << endl;
      acceleration = -3.5;
      steering_target = 0.0;
      steering_rate = 100.0;
      gear_location = apollo::canbus::Chassis::GEAR_NEUTRAL;
      simcar_status.current_driving_mode =
          apollo::canbus::Chassis::COMPLETE_MANUAL;
    } else {
      acceleration = control_msg->acceleration();
      steering_target = control_msg->steering_target();
      steering_rate = control_msg->steering_rate();
      gear_location = control_msg->gear_location();
      simcar_status.current_driving_mode =
          apollo::canbus::Chassis::COMPLETE_AUTO_DRIVE;
    }
    if (isnan(acceleration)) {
      acceleration = 0.0;
      cout << "acceleration is nan!" << endl;
    }
    if (isnan(steering_target)) {
      steering_target = 0.0;
      steering_rate = 0.0;
      cout << "steering_target is nan!" << endl;
    }
    fake_chassis(acceleration, steering_target, steering_rate, gear_location,
                 &simcar_status);
    PublishCanbus(&simcar_status);
    PublishLocalization(&simcar_status);

    sleep_for(chrono::milliseconds(1000 / FLAGS_feed_frequency));
  }
  cout << "Successfully fed proto files." << endl;
  return 0;
}

其中

fake_chassis(acceleration, steering_target, steering_rate, gear_location,
             &simcar_status);

函数是运动学模型，也是实现轻量级仿真的关键所在。

PublishCanbus(&simcar_status);

该段代码是发出底盘信息，供planning和control模块使用。

PublishLocalization(&simcar_status);

该段代码是发出定位信息，供planning和control模块使用。

DEFINE_string(fake_initial_localization_file,
              "apollo/modules/control/conf"
              "fake_initial_localization.pb.txt",
              "Used for set inital localization.");

该段代码的fake_initial_localization.pb.txt是给仿真模型指定初始信息，可在地图上任意选点。