自动驾驶循迹工具C++版

已于 2024-01-05 20:27:40 修改
阅读量228 收藏
点赞数
分类专栏： apollo研究 文章标签： 自动驾驶 c++
于 2023-09-10 13:20:08 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/xiaohukuzai/article/details/132789727
版权

背景

对于高校或自动驾驶爱好者，跑通整套自动驾驶框架难度较大，所需精力较多。前面博客已经介绍了如何使用仿真来数据pnc代码，但control模块对实车动力学和线控系统依赖比较大，不同车辆不同线控会有不同的性能表现。因此，本文实现学习人工驾驶数据的方式，来调试实车control模块，从而达到提高实车调试效率的目的。

实现原理

在人工驾驶下，基于车上搭载的惯性导航设备，底盘线控信号，录取驾驶数据；基于录取的驾驶数据，做成类似planning的trajectory轨迹，供control模块解耦调试。

代码示例

录取人工驾驶数据的脚本tracking_recorder.cc示例

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

#include "module/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/utility/time/time.h"
#include "modules/common/utility/util/string_util.h"

#include "modules/common_msgs/localization_msgs/localization.pb.h"
#include "modules/common_msgs/chassis_msgs/chassis.pb.h"
#include "modules/common_msgs/sal/dmi/dmi.pb.h"
#include "modules/common_msgs/basic_msgs/pnc_point.pb.h"

using namespace std;
using apollo::cyber::Rapolloer;
using apollo::cyber::Time;
using apollo::canbus::Chassis;
using apollo::common::TrajectoryPoint;
using apollo::common::math::Clamp;
using apollo::common::math::fast_sqrtf;
using apollo::common::time::Clock;
using apollo::common::util::StrCat;
using apollo::localization::LocalizationEstimate;
using std::this_thrapollo::sleep_for;

DEFINE_double(feed_frequency, 100.0,
              "Frequency with which protos are fed to control.");
DEFINE_bool(enable_speed_caculate_acc, false,
            "caculate acceleration base speed.");
DEFINE_bool(enable_wheel_speed, false, "use wheel speed or not.");
DEFINE_bool(enable_localization_acc, false,
            "use localization acceleration or not.");

struct TrackingRecorderStatus {
  double carx;
  double cary;
  double carz;
  double carspeed;
  double caracceleration;
  double carcurvature;
  double carcurvature_change_rate;
  double cartime;
  double cartheta;
  apollo::canbus::Chassis::GearPosition cargear;
  double cars;
  double throttle_percentage;
  double brake_percentage;
  double steering_percentage;
  double chassis_acc;
};
double carmax_steer_angle = 490;
double carsteer_ratio = 20;
double carwheel_base = 2.87;
double last_carcurvature = 0.0;
double speed_epsilon = 1e-9;
double last_carspeed = 0.0;
double pre_acc = 0.0;
void ProcessLogs(const TrackingRecorderStatus *trackingrecorder_status);
void WriteHapolloers(std::ofstream &file_stream);
std::string GetLogFileName();
std::ofstream rtk_recorder_log_file_;

std::string GetLogFileName() {
  time_t raw_time;
  char name_buffer[80];
  char sub_name_buffer[80];
  std::time(&raw_time);
  memset(sub_name_buffer, 0, sizeof(sub_name_buffer));
  // std::string str = FLAGS_control_log_path;
  std::string str = "/ssdata/log/control_log";
  strcpy(sub_name_buffer, str.data());
  strcat(sub_name_buffer, "/garage_%F_%H%M%S.csv");
  strftime(name_buffer, 80, sub_name_buffer, localtime(&raw_time));
  return std::string(name_buffer);
}

void WriteHapolloers(std::ofstream &file_stream) {
  file_stream << "carx,"
              << "cary,"
              << "carz,"
              << "carspeed,"
              << "caracceleration,"
              << "carcurvature,"
              << "carcurvature_change_rate,"
              << "cartime,"
              << "cartheta,"
              << "cargear,"
              << "cars,"
              << "throttle_percentage,"
              << "brake_percentage,"
              << "steering_percentage,"
              << "chassis_acc" << std::endl;
}

void ProcessLogs(const TrackingRecorderStatus *trackingrecorder_status) {
  const std::string log_str = StrCat(
      StrCat(trackingrecorder_status->carx, ",", trackingrecorder_status->cary, ",",
             trackingrecorder_status->carz, ",", trackingrecorder_status->carspeed, ","),
      StrCat(trackingrecorder_status->caracceleration, ",",
             trackingrecorder_status->carcurvature, ",",
             trackingrecorder_status->carcurvature_change_rate, ",",
             trackingrecorder_status->cartime, ","),
      StrCat(trackingrecorder_status->cartheta, ",", trackingrecorder_status->cargear,
             ",", trackingrecorder_status->cars, ",",
             trackingrecorder_status->throttle_percentage, ","),
      StrCat(trackingrecorder_status->brake_percentage, ",",
             trackingrecorder_status->steering_percentage, ",",
             trackingrecorder_status->chassis_acc));
  rtk_recorder_log_file_ << log_str << std::endl;
}

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

  rtk_recorder_log_file_.open(GetLogFileName());
  rtk_recorder_log_file_ << std::fixed;
  rtk_recorder_log_file_ << std::setprecision(6);
  WriteHapolloers(rtk_recorder_log_file_);

  shared_ptr<apollo::cyber::Node> node(
      apollo::cyber::CreateNode("rtk_recorder"));
  auto localization_ins_rapolloer = node->CreateRapolloer<LocalizationEstimate>(
      FLAGS_localization_global_result_topic, nullptr);
  auto chassis_rapolloer =
      node->CreateRapolloer<Chassis>(FLAGS_chassis_topic, nullptr);
  auto dmi_rapolloer = node->CreateRapolloer<apollo::sal::dmi::DMIRawDatas>(
      FLAGS_dmi_raw_topic, nullptr);
  TrackingRecorderStatus trackingrecorderstatus;
  // init trackingrecorderstatus
  trackingrecorderstatus.carx = 0.0;
  trackingrecorderstatus.cary = 0.0;
  trackingrecorderstatus.carz = 0.0;
  trackingrecorderstatus.carspeed = 0.0;
  trackingrecorderstatus.caracceleration = 0.0;
  trackingrecorderstatus.carcurvature = 0.0;
  trackingrecorderstatus.carcurvature_change_rate = 0.0;
  trackingrecorderstatus.cartime = 0.0;
  trackingrecorderstatus.cartheta = 0.0;
  trackingrecorderstatus.cargear = apollo::canbus::Chassis::GEAR_NEUTRAL;
  trackingrecorderstatus.cars = 0.0;
  trackingrecorderstatus.throttle_percentage = 0.0;
  trackingrecorderstatus.brake_percentage = 0.0;
  trackingrecorderstatus.steering_percentage = 0.0;
  trackingrecorderstatus.chassis_acc = 0.0;

  while (apollo::cyber::OK()) {
    double current_timestamp = Clock::NowInSeconds();
    localization_ins_rapolloer->Observe();
    chassis_rapolloer->Observe();
    dmi_rapolloer->Observe();
    const auto &localization_ins_msg =
        localization_ins_rapolloer->GetLatestObserved();
    const auto &chassis_msg = chassis_rapolloer->GetLatestObserved();
    const auto &dmi_msg = dmi_rapolloer->GetLatestObserved();
    trackingrecorderstatus.cartime = current_timestamp;

    if (localization_ins_msg == nullptr) {
      cout << "localization_ins_msg is empty!" << endl;
    } else {
      trackingrecorderstatus.carx = localization_ins_msg->global_localization().pose().position().x();
      trackingrecorderstatus.cary = localization_ins_msg->global_localization().pose().position().y();
      trackingrecorderstatus.carz = localization_ins_msg->global_localization().pose().position().z();
      trackingrecorderstatus.cartheta =
          localization_ins_msg->global_localization().pose().euler_angles().yaw();
          trackingrecorderstatus.caracceleration = fast_sqrtf(
      localization_ego_msg->local_localization().motion().linear_accel().x() *
      localization_ego_msg->local_localization().motion().linear_accel().x() +
      localization_ego_msg->local_localization().motion().linear_accel().y() *
      localization_ego_msg->local_localization().motion().linear_accel().y());
      if (trackingrecorderstatus.chassis_acc < 0) {
        trackingrecorderstatus.caracceleration =
              -trackingrecorderstatus.caracceleration;
      }
      cout << "Record log..." << endl;
    }

    if (chassis_msg == nullptr) {
      cout << "localization_ins_msg is empty!" << endl;
    } else {
      if (!FLAGS_enable_wheel_speed) {
        trackingrecorderstatus.carspeed = chassis_msg->speed_mps();
      }
      trackingrecorderstatus.chassis_acc = chassis_msg->acc_o();
      if (!FLAGS_enable_speed_caculate_acc && !FLAGS_enable_localization_acc) {
        trackingrecorderstatus.caracceleration = chassis_msg->acc_o();
      }
      trackingrecorderstatus.cargear = chassis_msg->gear_location();
      trackingrecorderstatus.throttle_percentage = 0.0;
      trackingrecorderstatus.brake_percentage = 0.0;
      trackingrecorderstatus.steering_percentage =
          chassis_msg->steering_percentage();
      if (fabs(trackingrecorderstatus.carspeed) < speed_epsilon &&
          fabs(last_carspeed) < speed_epsilon) {
        trackingrecorderstatus.carcurvature = 0.0;
      } else {
        trackingrecorderstatus.carcurvature =
            tan(chassis_msg->steering_percentage() / 100 * carmax_steer_angle *
                (M_PI / 180) / carsteer_ratio) /
            carwheel_base;
      }

      if (fabs(trackingrecorderstatus.carspeed) >= speed_epsilon) {
        trackingrecorderstatus.carcurvature_change_rate =
            (trackingrecorderstatus.carcurvature - last_carcurvature) /
            (trackingrecorderstatus.carspeed * 0.01);
      } else {
        trackingrecorderstatus.carcurvature_change_rate = 0.0;
      }
      if (FLAGS_enable_speed_caculate_acc) {
        trackingrecorderstatus.caracceleration =
            (trackingrecorderstatus.carspeed - last_carspeed) /
            (1 / FLAGS_feed_frequency);
      }
      trackingrecorderstatus.caracceleration = 
          apollo::common::math::Clamp(trackingrecorderstatus.caracceleration, -4.5, 4.0);
      trackingrecorderstatus.caracceleration =
          (pre_acc + trackingrecorderstatus.caracceleration) / 2;
      pre_acc = trackingrecorderstatus.caracceleration;

      last_carcurvature = trackingrecorderstatus.carcurvature;
      last_carspeed = trackingrecorderstatus.carspeed;
      trackingrecorderstatus.cars +=
          trackingrecorderstatus.carspeed * (1 / FLAGS_feed_frequency);
      cout << "Record localization_ins_msg..." << endl;
    }

    if (dmi_msg == nullptr) {
      cout << "dmi_msg is empty!" << endl;
    } else {
      if (FLAGS_enable_wheel_speed) {
        trackingrecorderstatus.carspeed =
            (dmi_msg->wheel_speed_lr().speed() + dmi_msg->wheel_speed_rr().speed()) / 2;
      }
      cout << "Record dmi_msg data..." << endl;
    }

    ProcessLogs(&trackingrecorderstatus);
    sleep_for(chrono::milliseconds(1000 / (int)FLAGS_feed_frequency));
  }
  return 0;
}

	```
	在tracking_recorder.cc录取好人工驾驶数据后，需要对数据进行分析，筛选出合理的数据，再给tracking_player.cc使用。
基于人工驾驶数据生成轨迹的tracking_player.cc示例:
```cpp
#include <cstdio>
#include <fstream>
#include <iostream>
#include <string>
#include <vector>

#include "corelib/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 "idl/topic/adapterFlags.h"
#include "modules\common\math\math_utils.h"
#include "modules/common/utility/time/time.h"

#include "modules/common_msgs/localization_msgs/localization.pb.h"
#include "modules/common_msgs/planning_msgs/planning.pb.h"
#include "modules/common_msgs/chassis_msgs/chassis.pb.h"
#include "modules/common_msgs/sal/dmi/dmi.pb.h"

using namespace std;
using apollo::common::math::Clamp;
using apollo::common::time::Clock;

DEFINE_int32(feed_frequency, 10,
             "Frequency with which protos are fed to control.");
DEFINE_double(speedmultiplier, 1.0, "Speed multiplier in percentage.");
DEFINE_double(replan_distance, 2.0,
              "replan when distance is larger than replan_distance.");
DEFINE_string(
    load_data_file,
    "/ssdata/log/control_log/garage_2023-09_10_093859.csv",
    "Used for load data from rtk_recorder.");
DEFINE_bool(enable_wheel_speed, false, "use wheel speed or not.");

#define SEARCH_INTERVAL 800
#define CHANGE_TO_COM false

struct TrackingPlayerStatus {
  double carx;
  double cary;
  double carz;
  double carspeed;
  double caracceleration;
  double carcurvature;
  double carcurvature_change_rate;
  double cartime;
  double cartheta;
  apollo::canbus::Chassis::GearPosition cargear;
  double cars;
  double throttle_percentage;
  double brake_percentage;
  double steering_percentage;
};
vector<vector<double>> test_data;
bool replan = false;
bool automode = false;
bool completepath = false;
bool estop = false;
double last_carspeed = 0.0;
double pre_acc = 0.0;
int closestpoint = 0;
double starttime = 0.0;
int start_ = 0;
int end_ = 0;
int data_size = 0;
double length = 4.986;
double back_edge_to_center = 1.1206;
int next_gear_switch_time(double start_, int length);
vector<vector<double>> LoadData(const char* strFilePath);
void restart(TrackingPlayerStatus* trackingplayer_status);
int closest_dist(TrackingPlayerStatus* trackingplayer_status);
int closest_time();
// void adc_position(apollo::pnc::planning::ADCTrajectory* planning_published_trajectory,
//                   const TrackingPlayerStatus* trackingplayer_status, const double time);

vector<vector<double>> LoadData(const char* strFilePath) {
  // Read CSV Data
  vector<vector<double>> vecData;
  ifstream inFile(strFilePath);
  string strLine;
  int nRowCount = 0;
  getline(inFile, strLine);  // skip first row data
  while (true) {
    getline(inFile, strLine);
    int nPos = strLine.find(",");
    if (nPos < 0) break;

    nRowCount++;
    vector<double> vecTmp;
    while (nPos > 0) {
      string strTmp = strLine.substr(0, nPos);
      double fTmp = atof(strTmp.c_str());
      vecTmp.push_back(fTmp);

      strLine.erase(0, nPos + 1);
      nPos = strLine.find(",");
    }
    double fTmp = atof(strLine.c_str());
    vecTmp.push_back(fTmp);
    vecData.push_back(vecTmp);
  }
  int nColCount = 0;
  if (nRowCount > 0) nColCount = vecData[0].size();

  double fData[nRowCount][nColCount];
  for (int nRow = 0; nRow < nRowCount; nRow++) {
    vector<double> vecTmp = vecData[nRow];
    vecTmp.resize(nColCount, 0);
    for (int nCol = 0; nCol < nColCount; nCol++) {
      fData[nRow][nCol] = vecTmp[nCol];
    }
    if (nRow < 5) cout << fData[nRow][0] << endl;
  }
  cout << "Data Row Count: " << nRowCount << " Column Count: " << nColCount << endl;
  return vecData;
}

void restart(TrackingPlayerStatus* trackingplayer_status) {
  replan = true;
  cout << "replan!" << endl;
  closestpoint = closest_dist(trackingplayer_status);
  start_ = std::max(closestpoint - 1, 0);
  starttime = Clock::NowInSeconds();
  end_ = next_gear_switch_time(start_, data_size);
  end_ = min(start_ + SEARCH_INTERVAL, data_size - 1);  // add by cnblog

  cout << "finish replan at time: " << starttime << '\t'
       << "closestpoint: " << closestpoint << endl;
}

int closest_dist(TrackingPlayerStatus* trackingplayer_status) {
  double shortest_dist_sqr = std::numeric_limits<double>::infinity();
  int search_start = std::max(start_ - (int)(SEARCH_INTERVAL / 2), 0);
  int search_end = std::min(start_ + (int)(SEARCH_INTERVAL / 2), data_size);
  int closest_dist_point = start_;
  for (int i = search_start; i < search_end; i++) {
    double dist_sqr = pow(trackingplayer_status->carx - test_data[i][0], 2) +
                      pow(trackingplayer_status->cary - test_data[i][1], 2);
    if (dist_sqr <= shortest_dist_sqr &&
        test_data[i][9] == trackingplayer_status->cargear) {  // 9: cargear
      closest_dist_point = i;
      shortest_dist_sqr = dist_sqr;
    }
  }
  if (shortest_dist_sqr == std::numeric_limits<double>::infinity()) {
    cout << "no trajectory point matches current gear position, check gear "
            "position"
         << endl;
    return closest_dist_point + 1;  // remain current start_ point
  }
  return closest_dist_point;
}

int closest_time() {
  double time_elapsed = Clock::NowInSeconds() - starttime;
  int closest_time = start_;
  double time_diff =
      test_data[closest_time][7] - test_data[closestpoint][7];  // 7: cartime
  while (time_diff < time_elapsed && closest_time < data_size - 1) {
    closest_time = closest_time + 1;
    time_diff = test_data[closest_time][7] - test_data[closestpoint][7];
  }
  return closest_time;
}

int next_gear_switch_time(double start_, int length) {
  int i = start_;
  for (; i < length; i++) {
    if (i < length - 1 &&
        (test_data[i][9] == 1 || test_data[i][9] == 2) &&  // 9: cargear
        test_data[i + 1][9] != test_data[i][9]) {
      i += 1;
      while (i < length && test_data[i][9] != 1 && test_data[i][9] != 2) {
        i += 1;
      }
      return i - 1;
    }
  }
  return min(i, length - 1);
}

void adc_position(apollo::pnc::planning::ADCTrajectory* planning_published_trajectory,
                  const TrackingPlayerStatus* trackingplayer_status, const double time) {
  planning_published_trajectory->mutable_debug()
      ->mutable_planning_data()
      ->mutable_adc_position()
      ->mutable_header()
      ->set_timestamp(time);
  planning_published_trajectory->mutable_debug()
      ->mutable_planning_data()
      ->mutable_adc_position()
      ->mutable_pose()
      ->mutable_position()
      ->set_x(trackingplayer_status->carx);
  planning_published_trajectory->mutable_debug()
      ->mutable_planning_data()
      ->mutable_adc_position()
      ->mutable_pose()
      ->mutable_position()
      ->set_y(trackingplayer_status->cary);
  planning_published_trajectory->mutable_debug()
      ->mutable_planning_data()
      ->mutable_adc_position()
      ->mutable_pose()
      ->mutable_position()
      ->set_z(trackingplayer_status->carz);
  planning_published_trajectory->mutable_debug()
      ->mutable_planning_data()
      ->mutable_adc_position()
      ->mutable_pose()
      ->mutable_euler_angles()
      ->set_roll(0.0);
  planning_published_trajectory->mutable_debug()
      ->mutable_planning_data()
      ->mutable_adc_position()
      ->mutable_pose()
      ->mutable_euler_angles()
      ->set_pitch(0.0);
  planning_published_trajectory->mutable_debug()
      ->mutable_planning_data()
      ->mutable_adc_position()
      ->mutable_pose()
      ->mutable_euler_angles()
      ->set_yaw(trackingplayer_status->cartheta);
}

int main(int argc, char** argv) {
  using apollo::cyber::Reader;
  using apollo::cyber::Time;
  using apollo::cyber::Writer;
  using apollo::cyber::common::GetProtoFromFile;
  using apollo::canbus::Chassis;
  using apollo::localization::LocalizationEstimate;
  using apollo::pnc::planning::ADCTrajectory;
  using google::protobuf::TextFormat;
  using google::protobuf::io::FileInputStream;
  using google::protobuf::io::ZeroCopyInputStream;
  using std::this_thread::sleep_for;

  google::ParseCommandLineFlags(&argc, &argv, true);
  apollo::cyber::Init(argv[0]);
  FLAGS_alsologtostderr = true;

  shared_ptr<apollo::cyber::Node> node(apollo::cyber::CreateNode("rtk_player"));
  auto planning_writer =
      node->CreateWriter<ADCTrajectory>(FLAGS_planning_trajectory_topic);
  auto localization_reader = node->CreateReader<LocalizationEstimate>(
      FLAGS_localization_global_result_topic, nullptr);
  auto chassis_reader =
      node->CreateReader<Chassis>(FLAGS_chassis_topic, nullptr);
  auto dmi_reader = node->CreateReader<apollo::sal::dmi::DMIRawDatas>(
      FLAGS_dmi_raw_topic, nullptr);

  std::string str = FLAGS_load_data_file;
  test_data = LoadData(str.data());
  data_size = test_data.size();

  TrackingPlayerStatus trackingplayerstatus;
  // init trackingplayerstatus
  trackingplayerstatus.carx = 0.0;
  trackingplayerstatus.cary = 0.0;
  trackingplayerstatus.carz = 0.0;
  trackingplayerstatus.carspeed = 0.0;
  trackingplayerstatus.caracceleration = 0.0;
  trackingplayerstatus.carcurvature = 0.0;
  trackingplayerstatus.carcurvature_change_rate = 0.0;
  trackingplayerstatus.cartime = 0.0;
  trackingplayerstatus.cartheta = 0.0;
  trackingplayerstatus.cargear = apollo::canbus::Chassis::GEAR_NEUTRAL;
  trackingplayerstatus.cars = 0.0;
  trackingplayerstatus.throttle_percentage = 0.0;
  trackingplayerstatus.brake_percentage = 0.0;
  trackingplayerstatus.steering_percentage = 0.0;

  int sequence_num = 0;
  while (apollo::cyber::OK()) {
    double current_timestamp = Clock::NowInSeconds();
    localization_reader->Observe();
    chassis_reader->Observe();
    dmi_reader->Observe();
    ADCTrajectory trajectory;
    const auto& localization_ins_msg = localization_reader->GetLatestObserved();
    const auto& chassis_msg = chassis_reader->GetLatestObserved();
    const auto& dmi_msg = dmi_reader->GetLatestObserved();
    if (localization_ins_msg == nullptr) {
      cout << "localization_ins_msg is empty!" << endl;
    } else {
      trackingplayerstatus.carx = localization_ins_msg->global_localization().pose().position().x();
      trackingplayerstatus.cary = localization_ins_msg->global_localization().pose().position().y();
      trackingplayerstatus.carz = localization_ins_msg->global_localization().pose().position().z();
      trackingplayerstatus.cartheta =
          localization_ins_msg->global_localization().pose().euler_angles().yaw();
    }
    if (dmi_msg == nullptr) {
      cout << "dmi_msg is empty!" << endl;
    } else {
      if (FLAGS_enable_wheel_speed) {
        trackingplayerstatus.carspeed =
            (dmi_msg->wheel_speed_lr().speed() + dmi_msg->wheel_speed_rr().speed()) / 2;
      }
      cout << "Record dmi_msg data..." << endl;
    }
    if (chassis_msg == nullptr) {
      cout << "chassis_msg is empty!" << endl;
    } else {
      automode = chassis_msg->driving_mode() ==
                         apollo::canbus::Chassis::COMPLETE_AUTO_DRIVE
                     ? true
                     : false;
      if (!FLAGS_enable_wheel_speed) {
        trackingplayerstatus.carspeed = chassis_msg->speed_mps();
      }
      trackingplayerstatus.cargear = chassis_msg->gear_location();
      trackingplayerstatus.cars += trackingplayerstatus.carspeed * 0.01;
      trackingplayerstatus.caracceleration =
          (trackingplayerstatus.carspeed - last_carspeed) / 0.01;
      trackingplayerstatus.caracceleration =  
          apollo::common::math::Clamp(trackingplayerstatus.caracceleration, -4.5, 4.0);
      trackingplayerstatus.caracceleration =
          (pre_acc + trackingplayerstatus.caracceleration) / 2;
      last_carspeed = trackingplayerstatus.carspeed;
    }

    trajectory.mutable_header()->set_timestamp(current_timestamp);
    trajectory.mutable_header()->mutable_module_info()->set_module_name("planning");
    trajectory.mutable_header()->mutable_module_info()->set_module_id(1);
    trajectory.mutable_header()->mutable_module_info()->set_sequence_num(sequence_num);
    trajectory.mutable_debug()
        ->mutable_planning_data()
        ->mutable_scenario()
        ->set_scenario_type(apollo::pnc::planning::ScenarioConfig::LKA);
    trajectory.set_is_replan(replan);
    trajectory.set_trajectory_type(apollo::pnc::planning::ADCTrajectory::NORMAL);
    sequence_num++;
    if (sequence_num <= 1 || !automode) {
      restart(&trackingplayerstatus);
    } else {
      replan = false;
      int timepoint = closest_time();
      int distpoint = closest_dist(&trackingplayerstatus);
      if (test_data[timepoint][9] == test_data[distpoint][9]) {  // 9: cargear
        start_ = max(min(timepoint, distpoint), 0);
      } else if (test_data[timepoint][9] == trackingplayerstatus.cargear) {
        start_ = timepoint;
      } else {
        start_ = distpoint;
      }
      end_ = next_gear_switch_time(start_, data_size);
      end_ = min(start_ + SEARCH_INTERVAL, data_size - 1);  // add by cnblog
      double xdiff_sqr = pow(test_data[timepoint][0] - trackingplayerstatus.carx, 2);
      double ydiff_sqr = pow(test_data[timepoint][1] - trackingplayerstatus.cary, 2);
      if (xdiff_sqr + ydiff_sqr >
          FLAGS_replan_distance * FLAGS_replan_distance) {
        cout << "distance is too far!" << endl;
        if (end_ - start_ <= 2 &&
            data_size - end_ <=
                2) {  // if finish the path from rtk_recorder, set estop!
          estop = true;
        } else {
          restart(&trackingplayerstatus);
          estop = false;
        }
      } else {
        replan = false;
      }
    }

    if (completepath) {
      start_ = 0;
      end_ = data_size - 1;
    }
    trajectory.set_total_path_length(test_data[end_][10] -
                                     test_data[start_][10]);
    // trajectory.set_gear(test_data[closest_time()][10]);
    trajectory.set_gear(Chassis::GEAR_DRIVE);
    trajectory.mutable_estop()->set_is_estop(estop);

    // add for visulization adc_position
    // adc_position(&trajectory, &trackingplayerstatus, current_timestamp);

    auto path = trajectory.mutable_debug()->mutable_planning_data()->add_path();
    path->set_name("DP_POLY_PATH_OPTIMIZER");

    for (int i = start_; i < end_; i++) {
      auto point = trajectory.add_trajectory_point();
      point->mutable_path_point()->set_x(test_data[i][0]);    // 0: carx
      point->mutable_path_point()->set_y(test_data[i][1]);    // 1: cary
      point->mutable_path_point()->set_z(test_data[i][2]);    // 2: carz
      point->set_v(test_data[i][3] * FLAGS_speedmultiplier);  // 3: carspeed
      point->set_a(test_data[i][4] *
                   FLAGS_speedmultiplier);  // 4: caracceleration
      point->mutable_path_point()->set_kappa(
          test_data[i][5]);  // 5: carcurvature
      point->mutable_path_point()->set_dkappa(
          test_data[i][6]);  // 6: carcurvature_change_rate
      point->mutable_path_point()->set_theta(test_data[i][8]);  // 8: cartheta
      point->mutable_path_point()->set_s(test_data[i][10]);     // 10: cars

      auto path_point = path->add_path_point();
      path_point->set_x(test_data[i][0]);       // 0: carx
      path_point->set_y(test_data[i][1]);       // 1: cary
      path_point->set_z(test_data[i][2]);       // 2: carz
      path_point->set_theta(test_data[i][8]);   // 8: cartheta
      path_point->set_kappa(test_data[i][5]);   // 5: carcurvature
      path_point->set_s(test_data[i][10]);      // 10: cars
      path_point->set_dkappa(test_data[i][6]);  // 6: carcurvature_change_rate
      path_point->set_ddkappa(0.0);

      if (CHANGE_TO_COM) {
        // translation vector length(length / 2 - back edge to center)
        cout << "CHANGE_TO_COM" << endl;
        point->mutable_path_point()->set_x(
            point->mutable_path_point()->x() +
            (length / 2 - back_edge_to_center) *
                std::cos(point->mutable_path_point()->theta()));
        point->mutable_path_point()->set_y(
            point->mutable_path_point()->y() +
            (length / 2 - back_edge_to_center) *
                std::sin(point->mutable_path_point()->theta()));
      }
      double time_diff =
          test_data[i][7] - test_data[closestpoint][7];  // 7: cartime
      point->set_relative_time(time_diff - (current_timestamp - starttime));
    }

    planning_writer->Write(trajectory);
    sleep_for(chrono::milliseconds(1000 / FLAGS_feed_frequency));
  }
  cout << "Successfully fed proto files." << endl;
  return 0;
}
书痴熊
关注
  • 0
    点赞
  • 踩
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
专栏目录
02、自动驾驶编程语言与工具C++篇
magicalmaster的博客
06-11 768
函数是可重复使用的代码块，可以帮助组织和管理代码。#include // 函数声明return 0;// 函数定义。
opencv循迹
12-03
在这个特定的项目中，“opencv循迹”是利用C++编程语言和OpenCV库实现的一种自动化导航技术，通常应用于机器人或自动驾驶车辆，使其能够沿着路径或标记进行追踪。 首先，我们要了解OpenCV中的直线检测算法。一种...
Apollo自动驾驶实践—从Apollo开发套件循迹开始
dreautumn的博客
08-15 3997
Apollo实践课程第0讲 导论第1讲：Apollo硬件连接集成—硬件介绍1.1 车辆循迹的硬件组成1.1.1计算单元1.1.2 定位系统1.1.3 CAN卡1.2 硬件连接与通讯简介第2讲 Apollo硬件连接集成—集成实操2.1 CAN卡的安装和要求2.1.1 准备工作2.1.2 拆装工控机2.2 将工控机安装在车辆上2.3 定位硬件的安装2.3.1 安装GPS天线2.3.2 安装GPS接收机和IMU2.4 4G路由器及显示器的安装2.5 连接通信接口第3讲 Apollo软件系统部署3.1 BIOS设置
自动驾驶开发中，openpilot的python&C++混合开发策略
Trb401012的博客
01-27 1028
这种策略利用了Python的快速迭代、简洁语法、丰富的库资源和强大的数据处理能力，加快了算法的原型设计和测试速度，同时保证了代码的可读性和可维护性。通过scons编译工具和Cython编码方式，openpilot实现了C++和Python代码的分离，保持了各自的纯净性，并允许开发者专注于自己擅长的领域。openpilot采用了scons编译工具（虽然cmake也能实现类似的功能，但可能需要更多的配置工作），这样的工具链使得C++代码保持纯净，即C++源文件中不包含Python代码，反之亦然。
【C++】Carla：开源仿真环境配置与使用示例
DevFrank的博客
06-29 4519
开源仿真环境配置与使用示例
Apollo进阶课程㊷丨Apollo实战——车辆与循迹驾驶能力实战
10点43的博客
03-06 2535
原创 阿波君 Apollo开发者社区 1月15日 循迹自动驾驶是指让车辆按照录制好的轨迹线进行自动驾驶，其涉及到自动驾驶中最基本的底盘线控能力、定位能力、控制能力，是自动驾驶系统的一个最小子集。 上周阿波君为大家详细介绍了「进阶课程㊶丨Apollo实战——本机演示实战」。 此课程介绍了Apollo项目的基本代码结构包括：Docker和Docs（主要放置一些文档）、Modules（核...
[运动控制算法]Pure Pursuit纯路径跟踪算法
xiewf的博客
03-19 7969
纯路径跟踪 Pure Pursuit是一种用于路径跟踪的控制算法。它通过计算角速度控制机器人从当前位置移到机器人前方的某个预瞄点。假定线速度是恒定的，当然可以随意更改机器人的线速度。该算法会根据机器人的当前位置在路径上移动预瞄点，直到路径的终点。可以想象成机器人不断追逐它前面的一个点。参数LookAheadDistance就决定将预瞄点放置有多远。 Pure Pursuit并不是传统的控制器，而是充当着跟踪算法的角色，用于跟随路径，航向点的信息是唯一的，可以指定所需的线速度及最大角速度。给定机器人的位姿（位
基于ldc1000的循迹小车.zip
04-29
它具有高分辨率和宽动态范围，适合在机器人、自动驾驶和工业自动化应用中使用。 2. **循迹算法**：小车需要一个算法来解析LDC1000提供的数据，判断当前位置和目标路径，然后调整车轮转速和方向以保持在路径上。这...
智能循迹判断.zip_STM32循迹小车_drawevc_寻迹小车灰度_循迹 stm32_灰度循迹小车
07-14
STM32是一款基于ARM Cortex-M内核的微控制器，广泛应用于嵌入式系统设计，尤其在机器人、自动化设备和智能小车等领域。...通过深入理解和实践这样的项目，开发者可以提升在物联网、自动驾驶和智能设备方面的技能。
基于stc32g12k128的开环循迹(含45度90度环岛十字T字十字终止线)小车(附完全代码)建议收藏
最新发布
06-06
1. **STC32G12K128微控制器**：STC32G12K128是STC公司生产的一款基于ARM Cortex-M0内核的单片机，拥有128KB的闪存和一定数量的RAM，适用于嵌入式控制系统，如自动驾驶小车。 2. **开环循迹**：开环控制系统是指系统...
WiFi通信和线性CCD摄像头循迹
12-26
在本文中，我们将深入探讨基于MSP432E401Y微控制器的WiFi通信与线性CCD摄像头循迹技术。...通过这样的系统，我们可以构建智能、自主导航的设备，广泛应用于机器人、自动驾驶小车、智能物流等领域。
开放式自动驾驶平台-C/C++开发
05-26
我们选择在这十年去月球去做其他事情，不是因为它们很容易，而是因为它们很难。 约翰·肯尼迪（John F. Kennedy），1962年欢迎来到Apollo的GitHub页面！ 阿波罗（Apollo）是一名高性能，灵活的建筑师，我们选择在这个十年中登月，做其他事情，不是因为它们很容易，而是因为它们很难。 约翰·肯尼迪（John F. Kennedy），1962年欢迎来到Apollo的GitHub页面！ Apollo是一种高性能，灵活的体系结构，可加速自动驾驶汽车的开发，测试和部署。 对于业务和合作伙伴关系，请访问我们的网站。 目录入门先决条件基本要求单个版本要求体系结构安装
最近项目小车自动循迹路线原码，C++编写
08-30
最近项目小车自动循迹路线原码，C++编写原码附上 需要。
智能小车循迹
07-09
智能小车循迹
自动驾驶Autoware源码
09-11
日本名古屋大学开源的自动驾驶源码：Autoware框架。适合做自动驾驶。
自动驾驶基于几何横向控制算法原理解析和实现（c++）
weixin_44149262的博客
08-08 484
自动驾驶横向控制算法原理解析，c++实现
C++中的自动驾驶与智能交通
naer_chongya的博客
01-20 479
此外，C++也具有内存管理和类型检查软件测试和验证：自动驾驶和智能交通系统的安全性和可靠性至关重要。借助C++提供的嵌入式系统开发、计算机视觉、机器学习、并发编程和并行计算、通信和网络、数据存储和处理、实时系统和安全性等技术，可以实现高效、安全和可靠的自动驾驶和智能交通系统。然而，需要注意的是，开发自动驾驶和智能交通系统是一个复杂的过程，需要综合考虑算法、硬件和实时性等方面的要求，才能达到预期的效果。数据隐私和安全：自动驾驶和智能交通系统涉及到大量的车辆数据和个人信息，对数据隐私和安全的保护非常重要。
自动驾驶之小巴自动循迹全过程详解
weixin_48657767的博客
03-02 1590
一、小巴底盘惯导数据接收、解析、上传 1.在serial.cpp中，实现惯导差分数据接收和解析 （1）惯导差分数据从哪里来？ 惯导差分数据可以参考之前串口收发和星网宇达官方sdk融合，融合之后就可以正常接收差分GPFPD数据，GPFPD数据举例如下所示： $GPFPD,2199,118326.980,264.364,0.635,1.458,32.0303810,120.9144455,16.85,-0.007,0.000,0.001,4.135,5,26,45*56 以上为差分定位GPFPD数据，里面包含
51循迹小车c++代码
11-15
根据提供的引用内容，我们可以了解到循迹小车使用的是STC89C52单片机作为系统的控制器，并且可以使用引用中提供的延时函数。下面是一个简单的51循迹小车C++代码示例： ```c++ #include <reg52.h> #define uchar unsigned char #define uint unsigned int sbit IN1 = P1^0; //定义IN1接口 sbit IN2 = P1^1; //定义IN2接口 sbit IN3 = P1^2; //定义IN3接口 sbit IN4 = P1^3; //定义IN4接口 void delay(uint x) //延时函数 { uint i, j; for(i = x; i > 0; i--) for(j = 110; j > 0; j--); } void main() { while(1) { if(P2 == 0x0f) //当循迹小车在黑线上时 { IN1 = 0; IN2 = 0; IN3 = 0; IN4 = 0; } else if(P2 == 0x07) //当循迹小车偏左时 { IN1 = 0; IN2 = 1; IN3 = 0; IN4 = 0; delay(100); } else if(P2 == 0x0b) //当循迹小车偏右时 { IN1 = 0; IN2 = 0; IN3 = 1; IN4 = 0; delay(100); } else if(P2 == 0x03) //当循迹小车完全偏左时 { IN1 = 0; IN2 = 1; IN3 = 0; IN4 = 1; delay(100); } else if(P2 == 0x0c) //当循迹小车完全偏右时 { IN1 = 0; IN2 = 1; IN3 = 1; IN4 = 0; delay(100); } } } ```

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值