Skip to content
This repository was archived by the owner on Jul 4, 2025. It is now read-only.

feat(dreamview): add a sim control feature in dreamview #14

Open
ChaoYangShi wants to merge 1 commit into
base: 8.0-dev
Choose a base branch
from
Open

feat(dreamview): add a sim control feature in dreamview #14

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 5 additions & 0 deletions modules/dreamview/backend/sim_control_manager/dynamic_model/perfect_control/BUILD
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,11 @@ cc_library(
"//modules/common_msgs/planning_msgs:planning_cc_proto",
"//modules/common_msgs/prediction_msgs:prediction_obstacle_cc_proto",
"//modules/common_msgs/prediction_msgs:scenario_cc_proto",
"//modules/common_msgs/control_msgs:control_cmd_cc_proto",
"//modules/control/controller:lat_controller",
"//modules/control/controller:lon_controller",
"//modules/control/common:dependency_injector",
"//modules/common_msgs/basic_msgs:error_code_cc_proto",
"@com_google_googletest//:gtest",
],
)
Expand Down
199 changes: 153 additions & 46 deletions ...reamview/backend/sim_control_manager/dynamic_model/perfect_control/sim_perfect_control.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -24,6 +24,9 @@
#include "modules/common/math/quaternion.h"
#include "modules/common/util/message_util.h"
#include "modules/common/util/util.h"
#include "modules/common_msgs/control_msgs/control_cmd.pb.h"
#include "modules/common_msgs/basic_msgs/error_code.pb.h"
#include "modules/control/common/dependency_injector.h"

namespace apollo {
namespace dreamview {
Expand Down Expand Up @@ -100,6 +103,11 @@ void SimPerfectControl::InitTimerAndIO() {
[this](const std::shared_ptr<PredictionObstacles> &obstacles) {
this->OnPredictionObstacles(obstacles);
});
chassis_reader_ = node_->CreateReader<Chassis>(
FLAGS_chassis_topic,
[this](const std::shared_ptr<Chassis> &chassis) {
this->OnChassis(chassis);
});

localization_writer_ =
node_->CreateWriter<LocalizationEstimate>(FLAGS_localization_topic);
Expand All @@ -121,6 +129,32 @@ void SimPerfectControl::Init(bool set_start_point,
if (set_start_point && !FLAGS_use_navigation_mode) {
InitStartPoint(start_point_attr["start_velocity"],
start_point_attr["start_acceleration"]);
// 初始化横纵控制器
lat_controller_ = std::make_unique<apollo::control::LatController>();
lon_controller_ = std::make_unique<apollo::control::LonController>();
ACHECK(lat_controller_ != nullptr);
ACHECK(lon_controller_ != nullptr);
std::string conf_file = "modules/control/conf/lateral_controller.conf";
apollo::control::ControlConf control_conf_;
injector_ = std::make_shared<apollo::control::DependencyInjector>();
if (cyber::common::GetProtoFromFile(conf_file, &control_conf_)) {
AINFO << "Lateral controller config loaded from " << conf_file;
const auto lat_conf = control_conf_;
lat_controller_->Init(injector_, &lat_conf);
} else {
AERROR << "Failed to load lateral controller config from " << conf_file;
}

std::string conf_file_lon = "modules/control/conf/longitudinal_controller.conf";
if (cyber::common::GetProtoFromFile(conf_file_lon, &control_conf_)) {
AINFO << "Longitudinal controller config loaded from " << conf_file_lon;
const auto lon_conf = control_conf_;
lon_controller_->Init(injector_, &lon_conf);
} else {
AERROR << "Failed to load longitudinal controller config from "
<< conf_file_lon;
}

}
}

Expand Down Expand Up @@ -325,6 +359,19 @@ void SimPerfectControl::OnPredictionObstacles(
send_dummy_prediction_ = obstacles->header().module_name() == "SimPrediction";
}

void SimPerfectControl::OnChassis(
const std::shared_ptr<Chassis> &chassis) {
std::lock_guard<std::mutex> lock(mutex_);

if (!enabled_) {
return;
}

if (chassis->engine_started()) {
chassis_writer_->Write(chassis);
}
}

void SimPerfectControl::Start() {
std::lock_guard<std::mutex> lock(mutex_);

Expand Down Expand Up @@ -392,65 +439,125 @@ void SimPerfectControl::RunOnce() {

TrajectoryPoint trajectory_point;
Chassis::GearPosition gear_position;
if (!PerfectControlModel(&trajectory_point, &gear_position)) {
apollo::control::ControlCommand control_command_;


// if (!PerfectControlModel(&trajectory_point, &gear_position)) {
// AERROR << "Failed to calculate next point with perfect control model";
// return;
// }

if (!LatControlModel(
localization_reader_->GetLatestObserved().get(),
chassis_reader_->GetLatestObserved().get(),
current_trajectory_.get(), &control_command_).ok() ||
!LongControlModel(
localization_reader_->GetLatestObserved().get(),
chassis_reader_->GetLatestObserved().get(),
current_trajectory_.get(), &control_command_).ok()) {
AERROR << "Failed to calculate next point with perfect control model";
return;
}
double L = 1.0;
auto v = chassis_reader_->GetLatestObserved()->speed_mps();
auto x = localization_reader_->GetLatestObserved()->pose().position().x();
auto y = localization_reader_->GetLatestObserved()->pose().position().y();
auto theta = localization_reader_->GetLatestObserved()->pose().heading();

x += v * std::cos(theta) * kSimControlIntervalMs / 100.0;
y += v * std::sin(theta) * kSimControlIntervalMs / 100.0;
trajectory_point.mutable_path_point()->set_x(x);
trajectory_point.mutable_path_point()->set_y(y);
theta += v / L * std::tan(control_command_.steering_target()) * kSimControlIntervalMs / 100.0;
trajectory_point.mutable_path_point()->set_theta(theta);
v += control_command_.acceleration() * kSimControlIntervalMs / 100.0;
trajectory_point.set_v(v);
gear_position = Chassis::GEAR_DRIVE;

PublishChassis(trajectory_point.v(), gear_position);
PublishLocalization(trajectory_point);
}

bool SimPerfectControl::PerfectControlModel(
TrajectoryPoint *point, Chassis::GearPosition *gear_position) {
// Result of the interpolation.
auto current_time = Clock::NowInSeconds();
const auto &trajectory = current_trajectory_->trajectory_point();
*gear_position = current_trajectory_->gear();

if (!received_planning_) {
prev_point_ = next_point_;
} else {
if (current_trajectory_->estop().is_estop() ||
next_point_index_ >= trajectory.size()) {
// Freeze the car when there's an estop or the current trajectory has
// been exhausted.
Freeze();
} else {
// Determine the status of the car based on received planning message.
while (next_point_index_ < trajectory.size() &&
current_time > trajectory.Get(next_point_index_).relative_time() +
current_trajectory_->header().timestamp_sec()) {
++next_point_index_;
}

if (next_point_index_ >= trajectory.size()) {
next_point_index_ = trajectory.size() - 1;
}

if (next_point_index_ == 0) {
AERROR << "First trajectory point is a future point!";
return false;
}

prev_point_index_ = next_point_index_ - 1;

next_point_ = trajectory.Get(next_point_index_);
prev_point_ = trajectory.Get(prev_point_index_);
// bool SimPerfectControl::PerfectControlModel(
// TrajectoryPoint *point, Chassis::GearPosition *gear_position) {
// // Result of the interpolation.
// auto current_time = Clock::NowInSeconds();
// const auto &trajectory = current_trajectory_->trajectory_point();
// *gear_position = current_trajectory_->gear();

// if (!received_planning_) {
// prev_point_ = next_point_;
// } else {
// if (current_trajectory_->estop().is_estop() ||
// next_point_index_ >= trajectory.size()) {
// // Freeze the car when there's an estop or the current trajectory has
// // been exhausted.
// Freeze();
// } else {
// // Determine the status of the car based on received planning message.
// while (next_point_index_ < trajectory.size() &&
// current_time > trajectory.Get(next_point_index_).relative_time() +
// current_trajectory_->header().timestamp_sec()) {
// ++next_point_index_;
// }

// if (next_point_index_ >= trajectory.size()) {
// next_point_index_ = trajectory.size() - 1;
// }

// if (next_point_index_ == 0) {
// AERROR << "First trajectory point is a future point!";
// return false;
// }

// prev_point_index_ = next_point_index_ - 1;

// next_point_ = trajectory.Get(next_point_index_);
// prev_point_ = trajectory.Get(prev_point_index_);
// }
// }

// if (current_time > next_point_.relative_time() +
// current_trajectory_->header().timestamp_sec()) {
// // Don't try to extrapolate if relative_time passes last point
// *point = next_point_;
// } else {
// *point = InterpolateUsingLinearApproximation(
// prev_point_, next_point_,
// current_time - current_trajectory_->header().timestamp_sec());
// }
// return true;
// }

common::Status SimPerfectControl::LatControlModel(
const localization::LocalizationEstimate *localization,
const canbus::Chassis *chassis,
const planning::ADCTrajectory *planning_published_trajectory,
apollo::control::ControlCommand *cmd) {
if (!planning_published_trajectory || planning_published_trajectory->trajectory_point().empty()) {
AERROR << "LatControlModel: trajectory is empty.";
return common::Status(apollo::common::ErrorCode::CONTROL_COMPUTE_ERROR, "Empty trajectory.");
}

auto state = lat_controller_->ComputeControlCommand(
localization, chassis, planning_published_trajectory, cmd);
return state;
}

if (current_time > next_point_.relative_time() +
current_trajectory_->header().timestamp_sec()) {
// Don't try to extrapolate if relative_time passes last point
*point = next_point_;
} else {
*point = InterpolateUsingLinearApproximation(
prev_point_, next_point_,
current_time - current_trajectory_->header().timestamp_sec());
common::Status SimPerfectControl::LongControlModel(
const localization::LocalizationEstimate *localization,
const canbus::Chassis *chassis,
const planning::ADCTrajectory *planning_published_trajectory,
apollo::control::ControlCommand *cmd) {
if (!planning_published_trajectory || planning_published_trajectory->trajectory_point().empty()) {
AERROR << "LongControlModel: trajectory is empty.";
return common::Status(apollo::common::ErrorCode::CONTROL_COMPUTE_ERROR, "Empty trajectory.");
}
auto state = lon_controller_->ComputeControlCommand(
localization, chassis, planning_published_trajectory, cmd);
return state;
}
return true;
}

void SimPerfectControl::PublishChassis(double cur_speed,
Chassis::GearPosition gear_position) {
Expand Down
29 changes: 26 additions & 3 deletions ...dreamview/backend/sim_control_manager/dynamic_model/perfect_control/sim_perfect_control.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,6 +33,10 @@
#include "modules/dreamview/backend/common/dreamview_gflags.h"
#include "modules/dreamview/backend/map/map_service.h"
#include "modules/dreamview/backend/sim_control_manager/core/sim_control_base.h"
#include "modules/control/controller/lat_controller.h"
#include "modules/control/controller/lon_controller.h"
#include "modules/common_msgs/control_msgs/control_cmd.pb.h"
// #include "modules/control/proto/control_conf.pb.h"

/**
* @namespace apollo::dreamview
Expand Down Expand Up @@ -93,13 +97,28 @@ class SimPerfectControl final : public SimControlBase {
void OnPredictionObstacles(
const std::shared_ptr<apollo::prediction::PredictionObstacles>
&obstacles);
void OnChassis(const std::shared_ptr<apollo::canbus::Chassis> &chassis);
std::shared_ptr<apollo::control::DependencyInjector> injector_;


/**
* @brief Predict the next trajectory point using perfect control model
*/
bool PerfectControlModel(
apollo::common::TrajectoryPoint *point,
apollo::canbus::Chassis::GearPosition *gear_position);
// bool PerfectControlModel(
// apollo::common::TrajectoryPoint *point,
// apollo::canbus::Chassis::GearPosition *gear_position);

common::Status LatControlModel(
const localization::LocalizationEstimate *localization,
const canbus::Chassis *chassis,
const planning::ADCTrajectory *planning_published_trajectory,
apollo::control::ControlCommand *cmd);

common::Status LongControlModel(
const localization::LocalizationEstimate *localization,
const canbus::Chassis *chassis,
const planning::ADCTrajectory *planning_published_trajectory,
apollo::control::ControlCommand *cmd);

void PublishChassis(double cur_speed,
apollo::canbus::Chassis::GearPosition gear_position);
Expand Down Expand Up @@ -149,6 +168,7 @@ class SimPerfectControl final : public SimControlBase {
navigation_reader_;
std::shared_ptr<cyber::Reader<apollo::prediction::PredictionObstacles>>
prediction_reader_;
std::shared_ptr<cyber::Reader<apollo::canbus::Chassis>> chassis_reader_;

std::shared_ptr<cyber::Writer<apollo::localization::LocalizationEstimate>>
localization_writer_;
Expand Down Expand Up @@ -193,6 +213,9 @@ class SimPerfectControl final : public SimControlBase {

common::PathPoint adc_position_;

std::unique_ptr<apollo::control::LatController> lat_controller_;
std::unique_ptr<apollo::control::LonController> lon_controller_;

// Linearize reader/timer callbacks and external operations.
std::mutex mutex_;

Expand Down