From dc9efa68157a74d1caf36243d07e63ba31a12845 Mon Sep 17 00:00:00 2001 From: scy Date: Tue, 15 Apr 2025 22:40:19 +0800 Subject: [PATCH] feat(dreamview): add a sim control feature in dreamview --- .../dynamic_model/perfect_control/BUILD | 5 + .../perfect_control/sim_perfect_control.cc | 199 ++++++++++++++---- .../perfect_control/sim_perfect_control.h | 29 ++- 3 files changed, 184 insertions(+), 49 deletions(-) diff --git a/modules/dreamview/backend/sim_control_manager/dynamic_model/perfect_control/BUILD b/modules/dreamview/backend/sim_control_manager/dynamic_model/perfect_control/BUILD index 5899a956d32..ec1b43a9d22 100644 --- a/modules/dreamview/backend/sim_control_manager/dynamic_model/perfect_control/BUILD +++ b/modules/dreamview/backend/sim_control_manager/dynamic_model/perfect_control/BUILD @@ -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", ], ) diff --git a/modules/dreamview/backend/sim_control_manager/dynamic_model/perfect_control/sim_perfect_control.cc b/modules/dreamview/backend/sim_control_manager/dynamic_model/perfect_control/sim_perfect_control.cc index 42a938c5480..4f4bc90c79b 100644 --- a/modules/dreamview/backend/sim_control_manager/dynamic_model/perfect_control/sim_perfect_control.cc +++ b/modules/dreamview/backend/sim_control_manager/dynamic_model/perfect_control/sim_perfect_control.cc @@ -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 { @@ -100,6 +103,11 @@ void SimPerfectControl::InitTimerAndIO() { [this](const std::shared_ptr &obstacles) { this->OnPredictionObstacles(obstacles); }); + chassis_reader_ = node_->CreateReader( + FLAGS_chassis_topic, + [this](const std::shared_ptr &chassis) { + this->OnChassis(chassis); + }); localization_writer_ = node_->CreateWriter(FLAGS_localization_topic); @@ -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(); + lon_controller_ = std::make_unique(); + 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(); + 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; + } + } } @@ -325,6 +359,19 @@ void SimPerfectControl::OnPredictionObstacles( send_dummy_prediction_ = obstacles->header().module_name() == "SimPrediction"; } +void SimPerfectControl::OnChassis( + const std::shared_ptr &chassis) { + std::lock_guard lock(mutex_); + + if (!enabled_) { + return; + } + + if (chassis->engine_started()) { + chassis_writer_->Write(chassis); + } +} + void SimPerfectControl::Start() { std::lock_guard lock(mutex_); @@ -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) { diff --git a/modules/dreamview/backend/sim_control_manager/dynamic_model/perfect_control/sim_perfect_control.h b/modules/dreamview/backend/sim_control_manager/dynamic_model/perfect_control/sim_perfect_control.h index a6769e80080..abe9aecc1cf 100644 --- a/modules/dreamview/backend/sim_control_manager/dynamic_model/perfect_control/sim_perfect_control.h +++ b/modules/dreamview/backend/sim_control_manager/dynamic_model/perfect_control/sim_perfect_control.h @@ -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 @@ -93,13 +97,28 @@ class SimPerfectControl final : public SimControlBase { void OnPredictionObstacles( const std::shared_ptr &obstacles); + void OnChassis(const std::shared_ptr &chassis); + std::shared_ptr 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); @@ -149,6 +168,7 @@ class SimPerfectControl final : public SimControlBase { navigation_reader_; std::shared_ptr> prediction_reader_; + std::shared_ptr> chassis_reader_; std::shared_ptr> localization_writer_; @@ -193,6 +213,9 @@ class SimPerfectControl final : public SimControlBase { common::PathPoint adc_position_; + std::unique_ptr lat_controller_; + std::unique_ptr lon_controller_; + // Linearize reader/timer callbacks and external operations. std::mutex mutex_;