diff --git a/modules/dreamview/backend/simulator/BUILD b/modules/dreamview/backend/simulator/BUILD new file mode 100644 index 00000000..d0c8ec99 --- /dev/null +++ b/modules/dreamview/backend/simulator/BUILD @@ -0,0 +1,140 @@ +load("@rules_cc//cc:defs.bzl", "cc_binary", "cc_library") + +package(default_visibility = ["//visibility:public"]) + +cc_library( + name = "simulator_base", + hdrs = [ + "base/entity.h", + ], + copts = ["-DMODULE_NAME=\\"dreamview\\""], + deps = [ + "//cyber", + ], +) + +cc_library( + name = "simulator_scenario", + srcs = [ + "scenario/parser.cc", + "scenario/manager.cc", + ], + hdrs = [ + "scenario/scenario.h", + "scenario/parser.h", + "scenario/manager.h", + ], + copts = ["-DMODULE_NAME=\\"dreamview\\""], + deps = [ + ":simulator_base", + ":simulator_entity_ego", + ":simulator_entity_env", + "//modules/dreamview/backend/map:map_service", + "@com_github_nlohmann_json//:json", + ], +) + +cc_library( + name = "simulator_entity_ego", + srcs = [ + "entity/ego/ego_model.cc", + ], + hdrs = [ + "entity/ego/ego_model_base.h", + "entity/ego/ego_model.h", + ], + copts = ["-DMODULE_NAME=\\"dreamview\\""], + deps = [ + ":simulator_base", + "//cyber", + "//modules/common_msgs/planning_msgs:planning_cc_pb", + "//modules/common_msgs/chassis_msgs:chassis_cc_pb", + "//modules/common_msgs/localization_msgs:localization_cc_pb", + "//modules/dreamview/backend/map:map_service", + ], +) + +cc_library( + name = "simulator_entity_env", + srcs = [ + "entity/env/obstacle_manager.cc", + "entity/env/traffic_light_manager.cc", + ], + hdrs = [ + "entity/env/obstacle_manager.h", + "entity/env/traffic_light_manager.h", + ], + copts = ["-DMODULE_NAME=\\"dreamview\\""], + deps = [ + ":simulator_base", + "//cyber", + "//modules/common_msgs/perception_msgs:perception_obstacle_cc_pb", + "//modules/common_msgs/perception_msgs:traffic_light_detection_cc_pb", + "//modules/dreamview/backend/map:map_service", + "@com_github_nlohmann_json//:json", + ], +) + +cc_library( + name = "simulator_core", + srcs = [ + "core/engine.cc", + ], + hdrs = [ + "core/engine.h", + ], + copts = ["-DMODULE_NAME=\\"dreamview\\""], + deps = [ + ":simulator_base", + ":simulator_scenario", + ":simulator_entity_ego", + ":simulator_entity_env", + "//cyber", + "//modules/dreamview/backend/map:map_service", + ], +) + +cc_library( + name = "simulator_evaluation", + srcs = [ + "evaluation/evaluator.cc", + "evaluation/metrics/max_sim_time_metric.cc", + ], + hdrs = [ + "evaluation/evaluation_manager.h", + "evaluation/evaluator.h", + "evaluation/evaluator_base.h", + "evaluation/metrics/metric_base.h", + "evaluation/metrics/max_sim_time_metric.h", + ], + copts = ["-DMODULE_NAME=\\"dreamview\\""], + deps = [ + ":simulator_base", + ], +) + +cc_library( + name = "simulation_manager", + srcs = ["simulation_manager.cc"], + hdrs = ["simulation_manager.h"], + copts = ["-DMODULE_NAME=\\"dreamview\\""], + deps = [ + ":simulator_core", + ":simulator_evaluation", + ":simulator_scenario", + "//modules/dreamview/backend/map:map_service", + "@com_github_nlohmann_json//:json", + ], +) + +cc_binary( + name = "simulator_main", + srcs = ["simulator_main.cc"], + copts = ["-DMODULE_NAME=\"dreamview\""], + deps = [ + ":simulation_manager", + "//cyber", + "//modules/dreamview/backend/map:map_service", + "@com_github_gflags_gflags//:gflags", + ], +) diff --git a/modules/dreamview/backend/simulator/base/entity.h b/modules/dreamview/backend/simulator/base/entity.h new file mode 100644 index 00000000..b7c7ae1c --- /dev/null +++ b/modules/dreamview/backend/simulator/base/entity.h @@ -0,0 +1,23 @@ +#pragma once + +namespace apollo { +namespace dreamview { + +class SimEntity { + public: + virtual ~SimEntity() = default; + + virtual bool Init() = 0; + + // dt: delta time in seconds + virtual void Step(double dt) = 0; + + // Reset the entity to its initial configuration + virtual void Reset() = 0; + + // Publish resulting data/messages to Cyber RT channels + virtual void Publish() = 0; +}; + +} // namespace dreamview +} // namespace apollo diff --git a/modules/dreamview/backend/simulator/base/frame_context.h b/modules/dreamview/backend/simulator/base/frame_context.h new file mode 100644 index 00000000..b268f196 --- /dev/null +++ b/modules/dreamview/backend/simulator/base/frame_context.h @@ -0,0 +1,53 @@ +#pragma once + +#include +#include + +#include "modules/common/proto/pnc_point.pb.h" + +namespace apollo { +namespace dreamview { + +// 障碍物状态快照 +struct ObstacleStatus { + int id; + double x; + double y; + double theta; + double v; + double length; + double width; + bool is_static; +}; + +// Ego 车辆状态快照 +struct EgoState { + double x = 0.0; + double y = 0.0; + double z = 0.0; + double heading = 0.0; + double v = 0.0; + double a = 0.0; + double kappa = 0.0; // 曲率 + double throttle = 0.0; + double brake = 0.0; + double steering_percentage = 0.0; +}; + +// 仿真帧上下文:作为 Evaluator 的输入 +struct FrameContext { + // 时间信息 + double dt = 0.0; + double sim_time_sec = 0.0; + size_t frame_index = 0; + + // 状态数据(使用指针避免大对象拷贝,由 Engine 保证生命周期) + const EgoState* ego_state = nullptr; + const std::vector* obstacles = nullptr; + + // 扩展:地图信息(可选,用于检测是否压线、超速等) + // MapService* map_service = nullptr; +}; + +} // namespace dreamview +} // namespace apollo diff --git a/modules/dreamview/backend/simulator/core/engine.cc b/modules/dreamview/backend/simulator/core/engine.cc new file mode 100644 index 00000000..067bb993 --- /dev/null +++ b/modules/dreamview/backend/simulator/core/engine.cc @@ -0,0 +1,134 @@ +#include "modules/dreamview/backend/simulator/core/engine.h" + +#include + +namespace apollo { +namespace dreamview { + +SimulationEngine::SimulationEngine(MapService* map_service) + : node_(cyber::CreateNode("simulation_engine")), + map_service_(map_service) {} + +SimulationEngine::~SimulationEngine() { Stop(); } + +bool SimulationEngine::Init() { + tick_timer_.reset(new cyber::Timer(10, [this]() { this->Tick(); }, false)); + return true; +} + +void SimulationEngine::AddEntity(std::shared_ptr entity) { + std::lock_guard lock(mutex_); + entities_.push_back(std::move(entity)); +} + +void SimulationEngine::ClearEntities() { + std::lock_guard lock(mutex_); + entities_.clear(); +} + +void SimulationEngine::Play() { + std::lock_guard lock(mutex_); + last_tick_time_ = cyber::Time::Now().ToSecond(); + sim_time_sec_ = 0.0; + tick_count_ = 0; + is_running_ = true; + if (tick_timer_) { + tick_timer_->Start(); + } +} + +void SimulationEngine::Stop() { + std::lock_guard lock(mutex_); + is_running_ = false; + if (tick_timer_) { + tick_timer_->Stop(); + } +} + +void SimulationEngine::Reset() { + Stop(); + std::lock_guard lock(mutex_); + sim_time_sec_ = 0.0; + tick_count_ = 0; + for (auto& entity : entities_) { + entity->Reset(); + } +} + +bool SimulationEngine::IsRunning() const { + std::lock_guard lock(mutex_); + return is_running_; +} + +uint64_t SimulationEngine::GetTickCount() const { + std::lock_guard lock(mutex_); + return tick_count_; +} + +double SimulationEngine::GetSimTimeSec() const { + std::lock_guard lock(mutex_); + return sim_time_sec_; +} + +void SimulationEngine::Tick() { + std::lock_guard lock(mutex_); + if (!is_running_) return; + + double current_time = cyber::Time::Now().ToSecond(); + double dt = current_time - last_tick_time_; + last_tick_time_ = current_time; + if (dt < 0.0) { + dt = 0.0; + } + sim_time_sec_ += dt; + ++tick_count_; + + // 1. Update calculation (Step) + for (auto& entity : entities_) { + entity->Step(dt); + } + + // 2. Evaluation: build FrameContext from entity snapshots and run evaluators + frame_context_cache_.dt = dt; + frame_context_cache_.sim_time_sec = sim_time_sec_; + frame_context_cache_.frame_index = tick_count_; + + // Reset caches + ego_state_cache_ = EgoState(); + obstacles_cache_.clear(); + frame_context_cache_.ego_state = nullptr; + frame_context_cache_.obstacles = nullptr; + + for (const auto& entity : entities_) { + // Try ego + if (!frame_context_cache_.ego_state) { + auto ego = std::dynamic_pointer_cast(entity); + if (ego) { + ego_state_cache_ = ego->GetState(); + frame_context_cache_.ego_state = &ego_state_cache_; + } + } + // Try obstacle manager + auto obs = std::dynamic_pointer_cast(entity); + if (obs) { + obstacles_cache_ = obs->GetObstacleStatuses(); + frame_context_cache_.obstacles = &obstacles_cache_; + } + } + + // Invoke Evaluate on evaluator entities + for (const auto& entity : entities_) { + auto evaluator = std::dynamic_pointer_cast(entity); + if (evaluator) { + evaluator->Evaluate(frame_context_cache_); + } + } + + // 3. Data output (Publish) + for (auto& entity : entities_) { + entity->Publish(); + } +} + +} // namespace dreamview +} // namespace apollo diff --git a/modules/dreamview/backend/simulator/core/engine.h b/modules/dreamview/backend/simulator/core/engine.h new file mode 100644 index 00000000..05af4854 --- /dev/null +++ b/modules/dreamview/backend/simulator/core/engine.h @@ -0,0 +1,65 @@ +#pragma once + +#include +#include +#include +#include + +#include "cyber/cyber.h" +#include "cyber/time/time.h" +#include "modules/dreamview/backend/map/map_service.h" +#include "modules/dreamview/backend/simulator/base/entity.h" +#include "modules/dreamview/backend/simulator/base/frame_context.h" +#include "modules/dreamview/backend/simulator/entity/ego/ego_model.h" +#include "modules/dreamview/backend/simulator/entity/env/obstacle_manager.h" +#include "modules/dreamview/backend/simulator/evaluation/evaluator.h" + +namespace apollo { +namespace dreamview { + +class SimulationEngine { + public: + SimulationEngine(MapService* map_service); + ~SimulationEngine(); + + bool Init(); + + std::shared_ptr GetNode() const { return node_; } + void AddEntity(std::shared_ptr entity); + + // Load a parsed Scenario (preferred) rather than depending on JSON in-engine. + // We keep this or let SimulationManager build and inject the environment + // objects. Actually, wait, since SimulationManager builds everything, we just + // AddEntity. But maybe let's keep LoadScenario for backward compatibility or + // refactor gracefully. Actually let's remove LoadScenario and do it all via + // SimManager. + void ClearEntities(); + + void Tick(); + + uint64_t GetTickCount() const; + double GetSimTimeSec() const; + + private: + void Tick(); + + std::shared_ptr node_; + std::unique_ptr tick_timer_; + MapService* map_service_; + + std::vector> entities_; + + double last_tick_time_ = 0.0; + double sim_time_sec_ = 0.0; + uint64_t tick_count_ = 0; + bool is_running_ = false; + mutable std::mutex mutex_; + + // Caches used to hold temporary snapshots passed to Evaluator + FrameContext frame_context_cache_; + EgoState ego_state_cache_; + std::vector obstacles_cache_; +}; + +} // namespace dreamview +} // namespace apollo diff --git a/modules/dreamview/backend/simulator/entity/ego/ego_model.cc b/modules/dreamview/backend/simulator/entity/ego/ego_model.cc new file mode 100644 index 00000000..1f2f7dc0 --- /dev/null +++ b/modules/dreamview/backend/simulator/entity/ego/ego_model.cc @@ -0,0 +1,18 @@ +#include "modules/dreamview/backend/simulator/entity/ego/ego_model_base.h" + +namespace apollo { +namespace dreamview { + +EgoModel::EgoModel(std::shared_ptr node, MapService* map_service) + : node_(node), map_service_(map_service) { + localization_writer_ = + node_->CreateWriter( + "/apollo/localization/pose"); + chassis_writer_ = + node_->CreateWriter("/apollo/canbus/chassis"); + routing_writer_ = node_->CreateWriter( + "/apollo/routing_request"); +} + +} // namespace dreamview +} // namespace apollo diff --git a/modules/dreamview/backend/simulator/entity/ego/ego_model.h b/modules/dreamview/backend/simulator/entity/ego/ego_model.h new file mode 100644 index 00000000..e309eb29 --- /dev/null +++ b/modules/dreamview/backend/simulator/entity/ego/ego_model.h @@ -0,0 +1,50 @@ +#pragma once + +#include +#include +#include + +#include "modules/common_msgs/chassis_msgs/chassis.pb.h" +#include "modules/common_msgs/localization_msgs/localization.pb.h" +#include "modules/common_msgs/routing_msgs/routing.pb.h" + +#include "cyber/cyber.h" +#include "modules/common/math/vec2d.h" +#include "modules/dreamview/backend/map/map_service.h" +#include "modules/dreamview/backend/simulator/base/entity.h" + +namespace apollo { +namespace dreamview { + +class EgoModel : public SimEntity { + public: + EgoModel(std::shared_ptr node, MapService* map_service); + virtual ~EgoModel() = default; + + // Set the vehicle pose directly (x, y, heading) and optional velocity. + virtual void Teleport(double x, double y, double heading, + double velocity = 0.0) = 0; + + virtual EgoState GetState() const = 0; + + virtual void RequestRouting( + const std::vector& waypoints) = 0; + + bool Init() override = 0; + void Step(double dt) override = 0; + void Reset() override = 0; + void Publish() override = 0; + + protected: + std::shared_ptr node_; + MapService* map_service_; + + std::shared_ptr> + localization_writer_; + std::shared_ptr> chassis_writer_; + std::shared_ptr> + routing_writer_; +}; + +} // namespace dreamview +} // namespace apollo diff --git a/modules/dreamview/backend/simulator/entity/ego/ego_perfect_model.cc b/modules/dreamview/backend/simulator/entity/ego/ego_perfect_model.cc new file mode 100644 index 00000000..0cdddd0b --- /dev/null +++ b/modules/dreamview/backend/simulator/entity/ego/ego_perfect_model.cc @@ -0,0 +1,194 @@ +#include "modules/dreamview/backend/simulator/entity/ego/ego_perfect_model.h" + +#include "modules/common/math/linear_interpolation.h" +#include "modules/common/math/quaternion.h" +#include "modules/common/util/message_util.h" + +namespace apollo { +namespace dreamview { + +EgoPerfectModel::EgoPerfectModel(std::shared_ptr node, + MapService* map_service) + : EgoModelBase(node, map_service) { + planning_reader_ = node_->CreateReader( + "/apollo/planning", + [this]( + const std::shared_ptr& trajectory) { + this->OnPlanning(trajectory); + }); +} + +void EgoPerfectModel::SetPose(double x, double y, double heading, + double velocity) { + std::lock_guard lock(mutex_); + current_x_ = x; + current_y_ = y; + current_heading_ = heading; + current_velocity_ = velocity; + current_acceleration_ = 0.0; + current_trajectory_ = nullptr; // Clear previous trajectory +} + +void EgoPerfectModel::RequestRouting( + const std::vector& waypoints) { + auto routing_request = std::make_shared(); + apollo::common::util::FillHeader("EgoPerfectModel", routing_request.get()); + + for (const auto& pt : waypoints) { + auto* waypoint = routing_request->add_waypoint(); + waypoint->mutable_pose()->set_x(pt.x()); + waypoint->mutable_pose()->set_y(pt.y()); + } + routing_writer_->Write(routing_request); +} + +void EgoPerfectModel::Reset() { + std::lock_guard lock(mutex_); + current_trajectory_ = nullptr; + current_velocity_ = 0.0; + current_acceleration_ = 0.0; +} + +void EgoPerfectModel::OnPlanning( + const std::shared_ptr& trajectory) { + std::lock_guard lock(mutex_); + current_trajectory_ = trajectory; +} + +bool EgoPerfectModel::Init() { return true; } + +void EgoPerfectModel::Step(double dt) { + (void)dt; + double current_time_sec; + std::shared_ptr trajectory; + { + std::lock_guard lock(mutex_); + trajectory = current_trajectory_; + current_time_sec = last_sim_time_sec_; + } + + apollo::common::TrajectoryPoint target_point; + + if (!trajectory || trajectory->trajectory_point_size() == 0) { + // No trajectory yet, but maintain the transform and publish localization + target_point.mutable_path_point()->set_x(current_x_); + target_point.mutable_path_point()->set_y(current_y_); + target_point.mutable_path_point()->set_theta(current_heading_); + target_point.set_v(0.0); + target_point.set_a(0.0); + } else { + // Find matched point based on time (relative to trajectory header) + double relative_time = current_time_sec - trajectory->header().timestamp_sec(); + + if (relative_time < trajectory->trajectory_point(0).relative_time()) { + target_point = trajectory->trajectory_point(0); + } else if (relative_time >= + trajectory + ->trajectory_point(trajectory->trajectory_point_size() - 1) + .relative_time()) { + target_point = + trajectory->trajectory_point(trajectory->trajectory_point_size() - 1); + } else { + // Linear interpolation + for (int i = 0; i < trajectory->trajectory_point_size() - 1; ++i) { + if (trajectory->trajectory_point(i).relative_time() <= relative_time && + trajectory->trajectory_point(i + 1).relative_time() > + relative_time) { + target_point = + apollo::common::math::InterpolateUsingLinearApproximation( + trajectory->trajectory_point(i), + trajectory->trajectory_point(i + 1), relative_time); + break; + } + } + } + // Update internal state + current_x_ = target_point.path_point().x(); + current_y_ = target_point.path_point().y(); + current_heading_ = target_point.path_point().theta(); + current_velocity_ = target_point.v(); + current_acceleration_ = target_point.a(); + } + + current_time_sec_ = current_time_sec; + target_point_ = target_point; +} + +void EgoPerfectModel::Publish() { + PublishLocalization(target_point_, current_time_sec_); + PublishChassis(target_point_, current_time_sec_); +} + +void EgoPerfectModel::PublishLocalization( + const apollo::common::TrajectoryPoint& point, double absolute_time) { + auto localization = + std::make_shared(); + apollo::common::util::FillHeader("EgoPerfectModel", localization.get()); + localization->mutable_header()->set_timestamp_sec(absolute_time); + + auto* pose = localization->mutable_pose(); + pose->mutable_position()->set_x(point.path_point().x()); + pose->mutable_position()->set_y(point.path_point().y()); + pose->mutable_position()->set_z(0.0); + pose->set_heading(point.path_point().theta()); + + pose->mutable_linear_velocity()->set_x(std::cos(point.path_point().theta()) * + point.v()); + pose->mutable_linear_velocity()->set_y(std::sin(point.path_point().theta()) * + point.v()); + pose->mutable_linear_acceleration()->set_x( + std::cos(point.path_point().theta()) * point.a()); + pose->mutable_linear_acceleration()->set_y( + std::sin(point.path_point().theta()) * point.a()); + + apollo::common::math::Quaternion quaternion = + apollo::common::math::HeadingToQuaternion(point.path_point().theta()); + pose->mutable_orientation()->set_qw(quaternion.w()); + pose->mutable_orientation()->set_qx(quaternion.x()); + pose->mutable_orientation()->set_qy(quaternion.y()); + pose->mutable_orientation()->set_qz(quaternion.z()); + + localization_writer_->Write(localization); +} + +void EgoPerfectModel::PublishChassis( + const apollo::common::TrajectoryPoint& point, double absolute_time) { + auto chassis = std::make_shared(); + apollo::common::util::FillHeader("EgoPerfectModel", chassis.get()); + chassis->mutable_header()->set_timestamp_sec(absolute_time); + + chassis->set_engine_started(true); + chassis->set_driving_mode(apollo::canbus::Chassis::COMPLETE_AUTO_DRIVE); + chassis->set_gear_location(apollo::canbus::Chassis::GEAR_DRIVE); + chassis->set_speed_mps(point.v()); + + if (point.path_point().kappa() > 0.0) { + chassis->set_steering_percentage(point.path_point().kappa() * 100.0); + } + + chassis_writer_->Write(chassis); +} + +void EgoPerfectModel::SetSimTime(double sim_time_sec) { + std::lock_guard lock(mutex_); + last_sim_time_sec_ = sim_time_sec; +} + +EgoState EgoPerfectModel::GetState() const { + std::lock_guard lock(mutex_); + EgoState s; + s.x = current_x_; + s.y = current_y_; + s.z = current_z_; + s.heading = current_heading_; + s.v = current_velocity_; + s.a = current_acceleration_; + s.kappa = target_point_.path_point().kappa(); + s.steering_percentage = (target_point_.path_point().kappa() > 0.0) + ? target_point_.path_point().kappa() * 100.0 + : 0.0; + return s; +} + +} // namespace dreamview +} // namespace apollo diff --git a/modules/dreamview/backend/simulator/entity/ego/ego_perfect_model.h b/modules/dreamview/backend/simulator/entity/ego/ego_perfect_model.h new file mode 100644 index 00000000..f7128f3a --- /dev/null +++ b/modules/dreamview/backend/simulator/entity/ego/ego_perfect_model.h @@ -0,0 +1,62 @@ +#pragma once + +#include +#include +#include +#include + +#include "modules/common_msgs/planning_msgs/planning.pb.h" + +#include "modules/dreamview/backend/simulator/base/frame_context.h" +#include "modules/dreamview/backend/simulator/entity/ego/ego_model.h" + +namespace apollo { +namespace dreamview { + +class EgoPerfectModel : public EgoModel { + public: + EgoPerfectModel(std::shared_ptr node, MapService* map_service); + ~EgoPerfectModel() override = default; + + bool Init() override; + + void SetPose(double x, double y, double heading, + double velocity = 0.0) override; + void RequestRouting( + const std::vector& waypoints) override; + void Step(double dt) override; + void Reset() override; + void Publish() override; + + // Return current ego snapshot for FrameContext + EgoState GetState() const override; + + private: + void OnPlanning( + const std::shared_ptr& trajectory); + void PublishLocalization(const apollo::common::TrajectoryPoint& point, + double absolute_time); + void PublishChassis(const apollo::common::TrajectoryPoint& point, + double absolute_time); + + std::shared_ptr> + planning_reader_; + + std::mutex mutex_; + std::shared_ptr current_trajectory_; + + // Ego status + apollo::common::TrajectoryPoint target_point_; + double current_x_ = 0.0; + double current_y_ = 0.0; + double current_z_ = 0.0; + double current_heading_ = 0.0; + double current_velocity_ = 0.0; + double current_acceleration_ = 0.0; + double current_time_sec_ = 0.0; + // Last simulation time notified by engine (seconds) + double last_sim_time_sec_ = 0.0; +}; + +} // namespace dreamview +} // namespace apollo diff --git a/modules/dreamview/backend/simulator/entity/env/obstacle_manager.cc b/modules/dreamview/backend/simulator/entity/env/obstacle_manager.cc new file mode 100644 index 00000000..d5a31e31 --- /dev/null +++ b/modules/dreamview/backend/simulator/entity/env/obstacle_manager.cc @@ -0,0 +1,192 @@ +#include "modules/dreamview/backend/simulator/entity/env/obstacle_manager.h" + +#include + +#include "modules/common/util/message_util.h" + +namespace apollo { +namespace dreamview { + +ObstacleManager::ObstacleManager(std::shared_ptr node, + MapService* map_service) + : node_(node), map_service_(map_service) { + perception_writer_ = + node_->CreateWriter( + "/apollo/perception/obstacles"); +} + +void ObstacleManager::Reset() { + std::lock_guard lock(mutex_); + static_obstacles_.clear(); + dynamic_obstacles_.clear(); + // Do not reset next_obstacle_id_ to avoid ID churn across resets. + start_time_ = 0.0; +} + +void ObstacleManager::LoadObstacles(const nlohmann::json& scenario_json) { + std::lock_guard lock(mutex_); + static_obstacles_.clear(); + dynamic_obstacles_.clear(); + start_time_ = 0.0; + + auto convert_to_double = [](const nlohmann::json& val) -> double { + if (val.is_number()) return val.get(); + if (val.is_string()) return std::stod(val.get()); + return 0.0; + }; + + if (scenario_json.contains("s")) { + const auto& s_array = scenario_json["s"]; + for (const auto& item : s_array) { + if (!item.contains("p") || item["p"].size() < 2) continue; + SimStaticObstacle obs; + obs.id = next_obstacle_id_++; + obs.x = convert_to_double(item["p"][0]); + obs.y = convert_to_double(item["p"][1]); + obs.heading = item.contains("r") ? convert_to_double(item["r"]) : 0.0; + obs.width = item.contains("w") ? convert_to_double(item["w"]) : 2.0; + obs.length = item.contains("h") ? convert_to_double(item["h"]) : 5.0; + static_obstacles_.push_back(obs); + } + } + + if (scenario_json.contains("d")) { + const auto& d_array = scenario_json["d"]; + for (const auto& item : d_array) { + if (!item.contains("p") || item["p"].size() < 2) continue; + if (!item.contains("v") || item["v"].size() < 2) continue; + + SimDynamicObstacle obs; + // Respect provided id if present to keep IDs stable for downstream + // modules + if (item.contains("id")) { + obs.id = static_cast(convert_to_double(item["id"])); + } else { + obs.id = next_obstacle_id_++; + } + // Expect dynamic obstacle positions in Cartesian (x,y). + obs.x = convert_to_double(item["p"][0]); + obs.y = convert_to_double(item["p"][1]); + obs.vx = convert_to_double(item["v"][0]); + obs.vy = convert_to_double(item["v"][1]); + obs.width = item.contains("w") ? convert_to_double(item["w"]) : 2.0; + obs.length = item.contains("h") ? convert_to_double(item["h"]) : 5.0; + obs.trigger_time = + item.contains("t") ? convert_to_double(item["t"]) : 0.0; + obs.is_active = false; + + dynamic_obstacles_.push_back(obs); + } + } +} + +void ObstacleManager::LoadFromScenario( + const apollo::dreamview::Scenario& scenario) { + std::lock_guard lock(mutex_); + // Do not reset next_obstacle_id_ to preserve ID stability + static_obstacles_.clear(); + dynamic_obstacles_.clear(); + + for (const auto& s : scenario.static_obstacles) { + SimStaticObstacle obs; + obs.id = (s.id != 0) ? s.id : next_obstacle_id_++; + obs.x = s.x; + obs.y = s.y; + obs.heading = s.heading; + obs.width = s.width; + obs.length = s.length; + static_obstacles_.push_back(obs); + } + + for (const auto& d : scenario.dynamic_obstacles) { + SimDynamicObstacle obs; + obs.id = (d.id != 0) ? d.id : next_obstacle_id_++; + obs.x = d.x; + obs.y = d.y; + obs.vx = d.vx; + obs.vy = d.vy; + obs.width = d.width; + obs.length = d.length; + obs.trigger_time = d.trigger_time; + obs.is_active = false; + dynamic_obstacles_.push_back(obs); + } +} + +bool ObstacleManager::Init() { return true; } + +void ObstacleManager::Step(double dt) { + std::lock_guard lock(mutex_); + if (dt < 0.0) { + dt = 0.0; + } + start_time_ += dt; + double elapsed_time = start_time_; + + for (auto& dyn_obs : dynamic_obstacles_) { + if (!dyn_obs.is_active && elapsed_time >= dyn_obs.trigger_time) { + dyn_obs.is_active = true; + } + if (dyn_obs.is_active) { + // Update Cartesian position + dyn_obs.x += dyn_obs.vx * dt; + dyn_obs.y += dyn_obs.vy * dt; + } + } +} + +void ObstacleManager::Publish() { + if (static_obstacles_.empty() && dynamic_obstacles_.empty()) { + return; + } + + auto perception_obstacles = + std::make_shared(); + apollo::common::util::FillHeader("ObstacleManager", + perception_obstacles.get()); + perception_obstacles->mutable_header()->set_timestamp_sec( + cyber::Time::Now().ToSecond()); + + // Add Static + for (const auto& obs : static_obstacles_) { + auto* perception_obstacle = perception_obstacles->add_perception_obstacle(); + perception_obstacle->set_id(obs.id); + perception_obstacle->mutable_position()->set_x(obs.x); + perception_obstacle->mutable_position()->set_y(obs.y); + perception_obstacle->set_theta(obs.heading); + perception_obstacle->set_length(obs.length); + perception_obstacle->set_width(obs.width); + perception_obstacle->set_height(2.0); + perception_obstacle->set_type( + apollo::perception::PerceptionObstacle::VEHICLE); + perception_obstacle->mutable_velocity()->set_x(0); + perception_obstacle->mutable_velocity()->set_y(0); + } + + // Add Dynamic (Frenet -> XY using MapService) + for (const auto& obs : dynamic_obstacles_) { + if (!obs.is_active) continue; + // Dynamic obstacles are stored in Cartesian; publish directly. + auto* p_obs = perception_obstacles->add_perception_obstacle(); + p_obs->set_id(obs.id); + p_obs->mutable_position()->set_x(obs.x); + p_obs->mutable_position()->set_y(obs.y); + // Heading: infer from velocity vector if available + double theta = 0.0; + if (std::abs(obs.vx) > 1e-6 || std::abs(obs.vy) > 1e-6) { + theta = std::atan2(obs.vy, obs.vx); + } + p_obs->set_theta(theta); + p_obs->set_length(obs.length); + p_obs->set_width(obs.width); + p_obs->set_height(2.0); + p_obs->set_type(apollo::perception::PerceptionObstacle::VEHICLE); + p_obs->mutable_velocity()->set_x(obs.vx); + p_obs->mutable_velocity()->set_y(obs.vy); + } + + perception_writer_->Write(perception_obstacles); +} + +} // namespace dreamview +} // namespace apollo diff --git a/modules/dreamview/backend/simulator/entity/env/obstacle_manager.h b/modules/dreamview/backend/simulator/entity/env/obstacle_manager.h new file mode 100644 index 00000000..0c0c8822 --- /dev/null +++ b/modules/dreamview/backend/simulator/entity/env/obstacle_manager.h @@ -0,0 +1,71 @@ +#pragma once + +#include +#include +#include + +#include "nlohmann/json.hpp" + +#include "modules/common_msgs/perception_msgs/perception_obstacle.pb.h" + +#include "cyber/cyber.h" +#include "modules/common/math/vec2d.h" +#include "modules/dreamview/backend/map/map_service.h" +#include "modules/dreamview/backend/simulator/base/entity.h" +#include "modules/dreamview/backend/simulator/scenario/scenario.h" + +namespace apollo { +namespace dreamview { + +struct SimStaticObstacle { + int id; + double x; + double y; + double heading; + double width; + double length; +}; + +struct SimDynamicObstacle { + int id; + // Use Cartesian coordinates (x,y) and velocities in XY frame (vx, vy). + double x; + double y; + double vx; + double vy; + double width; + double length; + double trigger_time; + bool is_active; +}; + +class ObstacleManager : public SimEntity { + public: + ObstacleManager(std::shared_ptr node, MapService* map_service); + ~ObstacleManager() = default; + + void LoadFromScenario(const apollo::dreamview::Scenario& scenario); + + bool Init() override; + void Step(double dt) override; + void Reset() override; + void Publish() override; + + std::vector GetObstacleStatuses() const; + + private: + std::shared_ptr node_; + MapService* map_service_; + std::shared_ptr> + perception_writer_; + + std::mutex mutex_; + std::vector static_obstacles_; + std::vector dynamic_obstacles_; + + double start_time_ = 0.0; + int next_obstacle_id_ = 1000; +}; + +} // namespace dreamview +} // namespace apollo diff --git a/modules/dreamview/backend/simulator/entity/env/traffic_light_manager.cc b/modules/dreamview/backend/simulator/entity/env/traffic_light_manager.cc new file mode 100644 index 00000000..8ea356be --- /dev/null +++ b/modules/dreamview/backend/simulator/entity/env/traffic_light_manager.cc @@ -0,0 +1,90 @@ +#include "modules/dreamview/backend/simulator/entity/env/traffic_light_manager.h" + +#include "modules/common/util/message_util.h" + +using apollo::common::util::FillHeader; + +namespace apollo { +namespace dreamview { + +TrafficLightManager::TrafficLightManager(std::shared_ptr node) + : node_(node) {} + +void TrafficLightManager::LoadFromScenario(const Scenario& scenario) { + std::lock_guard lock(mutex_); + lights_.clear(); + for (const auto& t : scenario.traffic_lights) { + SimTrafficLight lt; + lt.id = (t.id != 0) ? t.id : 0; + lt.x = t.x; + lt.y = t.y; + lt.state = t.state; + lt.trigger_time = t.trigger_time; + lt.is_active = false; + lights_.push_back(lt); + } + // Create writer for TrafficLightDetection; topic name follows convention. + if (node_) { + tl_writer_ = node_->CreateWriter( + "/perception/traffic_light_detection"); + } +} + +bool TrafficLightManager::Init() { return true; } + +void TrafficLightManager::Step(double dt) override { + std::lock_guard lock(mutex_); + elapsed_time_sec_ += dt; + for (auto& light : lights_) { + if (!light.is_active && elapsed_time_sec_ >= light.trigger_time) { + light.is_active = true; + } + // 这里可以增加简单的红绿黄切换逻辑 + } +} + +void TrafficLightManager::Publish() { + std::lock_guard lock(mutex_); + bool has_active = false; + for (auto& lt : lights_) { + if (lt.is_active) has_active = true; + } + + // Publish traffic light detection if there is any active light + if (tl_writer_ && has_active) { + auto msg = std::make_shared(); + FillHeader("TrafficLightManager", msg.get()); + msg->mutable_header()->set_timestamp_sec(cyber::Time::Now().ToSecond()); + for (const auto& lt : lights_) { + if (!lt.is_active) continue; + auto* out = msg->add_traffic_light(); + out->set_id(lt.id); + // Map internal state to proto enum: 1=red,2=yellow,3=green + switch (lt.state) { + case 1: + out->set_color(apollo::perception::TrafficLight::RED); + break; + case 2: + out->set_color(apollo::perception::TrafficLight::YELLOW); + break; + case 3: + out->set_color(apollo::perception::TrafficLight::GREEN); + break; + default: + out->set_color(apollo::perception::TrafficLight::UNKNOWN); + } + out->set_position_x(lt.x); + out->set_position_y(lt.y); + } + tl_writer_->Write(msg); + } +} + +void TrafficLightManager::Reset() { + std::lock_guard lock(mutex_); + lights_.clear(); + elapsed_time_sec_ = 0.0; +} + +} // namespace dreamview +} // namespace apollo diff --git a/modules/dreamview/backend/simulator/entity/env/traffic_light_manager.h b/modules/dreamview/backend/simulator/entity/env/traffic_light_manager.h new file mode 100644 index 00000000..0349f975 --- /dev/null +++ b/modules/dreamview/backend/simulator/entity/env/traffic_light_manager.h @@ -0,0 +1,49 @@ +#pragma once + +#include +#include +#include + +#include "modules/common_msgs/perception_msgs/traffic_light_detection.pb.h" + +#include "cyber/cyber.h" +#include "modules/dreamview/backend/simulator/base/entity.h" +#include "modules/dreamview/backend/simulator/scenario/scenario.h" + +namespace apollo { +namespace dreamview { + +class TrafficLightManager : public SimEntity { + public: + explicit TrafficLightManager(std::shared_ptr node); + ~TrafficLightManager() = default; + + void LoadFromScenario(const Scenario& scenario); + bool Init() override; + void Step(double dt) override; + void Reset() override; + void Publish() override; + + std::vector GetTrafficLights() const; + + private: + std::mutex mutex_; + + struct SimTrafficLight { + int id = 0; + double x = 0.0; + double y = 0.0; + int state = 0; + double trigger_time = 0.0; + bool is_active = false; + }; + + std::vector lights_; + std::shared_ptr node_; + double elapsed_time_sec_ = 0.0; + std::shared_ptr> + tl_writer_; +}; + +} // namespace dreamview +} // namespace apollo diff --git a/modules/dreamview/backend/simulator/evaluation/evaluator.cc b/modules/dreamview/backend/simulator/evaluation/evaluator.cc new file mode 100644 index 00000000..daa24691 --- /dev/null +++ b/modules/dreamview/backend/simulator/evaluation/evaluator.cc @@ -0,0 +1,72 @@ +#include "modules/dreamview/backend/simulator/evaluation/evaluation_manager.h" + +namespace apollo { +namespace dreamview { + +void EvaluationManager::AddMetric(std::unique_ptr metric) { + std::lock_guard lock(mutex_); + metrics_.push_back(std::move(metric)); +} + +void EvaluationManager::Step(double dt) { + std::lock_guard lock(mutex_); + if (dt < 0.0) { + dt = 0.0; + } + elapsed_time_sec_ += dt; + + // Step 里只做时间推进,不建议在这里调 Evaluate + // 因为此时其他 Entity 的 Step 可能还没跑完,数据不全 +} + +void EvaluationManager::Reset() { + std::lock_guard lock(mutex_); + elapsed_time_sec_ = 0.0; + has_terminal_event_ = false; + terminal_reason_.clear(); + + for (auto& metric : metrics_) { + metric->Reset(); + } +} + +void EvaluationManager::Publish() { + // Aggregate scores or metrics from all evaluators and publish them via Cyber + // if necessary To be implemented as the evaluation system grows +} + +bool EvaluationManager::HasTerminalEvent() const { + std::lock_guard lock(mutex_); + return has_terminal_event_; +} + +std::string EvaluationManager::TerminalReason() const { + std::lock_guard lock(mutex_); + return terminal_reason_; +} + +EvaluationManager::Summary EvaluationManager::GetSummary() const { + std::lock_guard lock(mutex_); + Summary summary; + summary.has_terminal_event = has_terminal_event_; + summary.terminal_reason = terminal_reason_; + summary.elapsed_time_sec = elapsed_time_sec_; + summary.metric_count = metrics_.size(); + return summary; +} + +void EvaluationManager::UpdateTerminalStatusLocked() { + if (has_terminal_event_) { + return; + } + for (const auto& metric : metrics_) { + if (metric->IsTerminal()) { + has_terminal_event_ = true; + terminal_reason_ = metric->TerminalReason(); + return; + } + } +} + +} // namespace dreamview +} // namespace apollo diff --git a/modules/dreamview/backend/simulator/evaluation/evaluator.h b/modules/dreamview/backend/simulator/evaluation/evaluator.h new file mode 100644 index 00000000..b647b8f0 --- /dev/null +++ b/modules/dreamview/backend/simulator/evaluation/evaluator.h @@ -0,0 +1,64 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "modules/dreamview/backend/simulator/base/entity.h" +#include "modules/dreamview/backend/simulator/evaluation/metrics/metric.h" + +namespace apollo { +namespace dreamview { + +class Evaluator : public SimEntity { + public: + struct Summary { + bool has_terminal_event = false; + std::string terminal_reason; + double elapsed_time_sec = 0.0; + std::size_t evaluator_count = 0; + std::size_t metric_count = 0; + }; + + Evaluator() = default; + ~Evaluator() override = default; + + bool Init() override { return true; } + void AddMetric(std::unique_ptr metric); + void Evaluate(const FrameContext& context) { + std::lock_guard lock(mutex_); + if (has_terminal_event_) return; // 已停止则不再计算 + + for (auto& metric : metrics_) { + metric->Evaluate(context); + if (metric->IsTerminal()) { + has_terminal_event_ = true; + terminal_reason_ = metric->Name() + ": " + metric->TerminalReason(); + break; + } + } + } + + void Step(double dt) override; + void Reset() override; + void Publish() override; + + bool HasTerminalEvent() const; + std::string TerminalReason() const; + Summary GetSummary() const; + + private: + void UpdateTerminalStatusLocked(); + + mutable std::mutex mutex_; + double elapsed_time_sec_ = 0.0; + bool has_terminal_event_ = false; + std::string terminal_reason_; + + std::vector> metrics_; +}; + +} // namespace dreamview +} // namespace apollo diff --git a/modules/dreamview/backend/simulator/evaluation/metrics/max_sim_time_metric.cc b/modules/dreamview/backend/simulator/evaluation/metrics/max_sim_time_metric.cc new file mode 100644 index 00000000..15239d9e --- /dev/null +++ b/modules/dreamview/backend/simulator/evaluation/metrics/max_sim_time_metric.cc @@ -0,0 +1,23 @@ +#include "modules/dreamview/backend/simulator/evaluation/metrics/max_sim_time_metric.h" + +namespace apollo { +namespace dreamview { + +void MaxSimTimeMetric::Evaluate(const FrameContext& context) { + if (is_terminal_) { + return; + } + + if (context.sim_time_sec >= max_sim_time_sec_) { + is_terminal_ = true; + terminal_reason_ = "Simulation time limit exceeded."; + } +} + +void MaxSimTimeMetric::Reset() { + is_terminal_ = false; + terminal_reason_.clear(); +} + +} // namespace dreamview +} // namespace apollo diff --git a/modules/dreamview/backend/simulator/evaluation/metrics/max_sim_time_metric.h b/modules/dreamview/backend/simulator/evaluation/metrics/max_sim_time_metric.h new file mode 100644 index 00000000..9a8b39d5 --- /dev/null +++ b/modules/dreamview/backend/simulator/evaluation/metrics/max_sim_time_metric.h @@ -0,0 +1,30 @@ +#pragma once + +#include + +#include "modules/dreamview/backend/simulator/evaluation/metrics/metric.h" + +namespace apollo { +namespace dreamview { + +class MaxSimTimeMetric : public Metric { + public: + explicit MaxSimTimeMetric(double max_sim_time_sec) + : max_sim_time_sec_(max_sim_time_sec) {} + ~MaxSimTimeMetric() override = default; + + std::string Name() const override { return "max_sim_time"; } + void Evaluate(const FrameContext& context) override; + void Reset() override; + + bool IsTerminal() const override { return is_terminal_; } + std::string TerminalReason() const override { return terminal_reason_; } + + private: + double max_sim_time_sec_ = 0.0; + bool is_terminal_ = false; + std::string terminal_reason_; +}; + +} // namespace dreamview +} // namespace apollo diff --git a/modules/dreamview/backend/simulator/evaluation/metrics/metric.h b/modules/dreamview/backend/simulator/evaluation/metrics/metric.h new file mode 100644 index 00000000..3b309044 --- /dev/null +++ b/modules/dreamview/backend/simulator/evaluation/metrics/metric.h @@ -0,0 +1,24 @@ +#pragma once + +#include + +namespace apollo { +namespace dreamview { + +class Metric { + public: + virtual ~Metric() = default; + virtual std::string Name() const = 0; + + // 使用统一的 Context + virtual void Evaluate(const FrameContext& context) = 0; + virtual void Reset() = 0; + + virtual bool IsTerminal() const = 0; + virtual std::string TerminalReason() const = 0; + + virtual double GetValue() const { return 0.0; } +}; + +} // namespace dreamview +} // namespace apollo diff --git a/modules/dreamview/backend/simulator/scenario/manager.cc b/modules/dreamview/backend/simulator/scenario/manager.cc new file mode 100644 index 00000000..15748fe9 --- /dev/null +++ b/modules/dreamview/backend/simulator/scenario/manager.cc @@ -0,0 +1,128 @@ +#include "modules/dreamview/backend/simulator/scenario/manager.h" + +#include +#include + +#include "nlohmann/json.hpp" + +#include "modules/dreamview/backend/simulator/entity/ego/ego_model.h" +#include "modules/dreamview/backend/simulator/entity/ego/ego_perfect_model.h" +#include "modules/dreamview/backend/simulator/entity/env/obstacle_manager.h" +#include "modules/dreamview/backend/simulator/entity/env/traffic_light_manager.h" +#include "modules/dreamview/backend/simulator/scenario/parser.h" + +namespace apollo { +namespace dreamview { + +bool ScenarioManager::LoadScenario(const std::string& scenario_file) { + std::ifstream ifs(scenario_file); + if (!ifs.is_open()) { + AERROR << "Failed to open scenario file: " << scenario_file; + return false; + } + + nlohmann::json scenario_json; + try { + ifs >> scenario_json; + } catch (const nlohmann::json::parse_error& e) { + AERROR << "Failed to parse scenario file: " << scenario_file + << ", reason: " << e.what(); + return false; + } + + Scenario scenario; + if (!ScenarioParser::FromJson(scenario_json, &scenario)) { + AERROR << "Failed to convert scenario JSON: " << scenario_file; + return false; + } + + return LoadScenarioData(scenario); +} + +bool ScenarioManager::LoadScenarioData(const Scenario& scenario) { + if (!node_ || map_service_ == nullptr) { + AERROR << "ScenarioManager is not initialized with node/map service"; + return false; + } + + // 1. Instantiate Ego + auto ego_model = std::make_shared(node_, map_service_); + if (!ego_model->Init()) { + AERROR << "Failed to init ego model"; + return false; + } + + if (!scenario.ego_waypoints.empty()) { + if (scenario.ego_waypoints.size() >= 2) { + const auto& start = scenario.ego_waypoints[0]; + const auto& next = scenario.ego_waypoints[1]; + double heading = std::atan2(next.y() - start.y(), next.x() - start.x()); + ego_model->SetPose(start.x(), start.y(), heading, 0.0); + } else { + const auto& start = scenario.ego_waypoints[0]; + ego_model->SetPose(start.x(), start.y(), 0.0, 0.0); + } + ego_model->RequestRouting(scenario.ego_waypoints); + } + + ego_ = ego_model; + + // 2. Instantiate ObstacleManager and TrafficLightManager as entities + auto obstacle_manager = + std::make_shared(node_, map_service_); + if (!obstacle_manager->Init()) { + AERROR << "Failed to init obstacle manager"; + return false; + } + obstacle_manager->LoadFromScenario(scenario); + + auto traffic_light_manager = std::make_shared(node_); + if (!traffic_light_manager->Init()) { + AERROR << "Failed to init traffic light manager"; + return false; + } + traffic_light_manager->LoadFromScenario(scenario); + + entities_.clear(); + entities_.push_back(obstacle_manager); + entities_.push_back(traffic_light_manager); + + return true; +} + +bool ScenarioManager::LoadScenarioData(const Scenario& scenario) { + // 1. 清理旧数据 + entities_.clear(); + ego_ = nullptr; + + // 2. 实例化 Ego (工厂模式) + if (scenario.ego_config.type == "PERFECT_CONTROL") { + ego_ = std::make_shared(node_, map_service_); + } else { + // AERROR << "Unsupported ego type: " << scenario.ego_config.type; + return false; + } + + // 初始化 Ego 状态 + ego_->Init(); + ego_->Teleport(scenario.ego_config.x, scenario.ego_config.y, + scenario.ego_config.heading, scenario.ego_config.v); + + // 3. 实例化并配置障碍物 + auto obstacle_manager = + std::make_shared(node_, map_service_); + obstacle_manager->Init(); + obstacle_manager->LoadFromScenario(scenario); // 内部解析 scenario.obstacles + + // 4. 汇总到实体列表 + entities_.push_back(ego_); + entities_.push_back(obstacle_manager); + + // 5. 实例化红绿灯、评估器等并加入 entities_ + // ... + + return true; +} + +} // namespace dreamview +} // namespace apollo diff --git a/modules/dreamview/backend/simulator/scenario/manager.h b/modules/dreamview/backend/simulator/scenario/manager.h new file mode 100644 index 00000000..57b56e33 --- /dev/null +++ b/modules/dreamview/backend/simulator/scenario/manager.h @@ -0,0 +1,39 @@ +#pragma once + +#include +#include +#include + +#include "cyber/cyber.h" +#include "modules/dreamview/backend/map/map_service.h" +#include "modules/dreamview/backend/simulator/base/entity.h" +#include "modules/dreamview/backend/simulator/entity/ego/ego_model_base.h" +#include "modules/dreamview/backend/simulator/scenario/scenario.h" + +namespace apollo { +namespace dreamview { + +class ScenarioManager { + public: + ScenarioManager(std::shared_ptr node, MapService* map_service) + : node_(std::move(node)), map_service_(map_service) {} + ~ScenarioManager() = default; + + bool LoadScenario(const std::string& scenario_file); + bool LoadScenarioData(const Scenario& scenario); + + std::shared_ptr GetEgo() const { return ego_; } + + const std::vector>& GetEntities() const { + return entities_; + } + + private: + std::shared_ptr node_; + MapService* map_service_ = nullptr; + std::shared_ptr ego_; + std::vector> entities_; +}; + +} // namespace dreamview +} // namespace apollo diff --git a/modules/dreamview/backend/simulator/scenario/parser.cc b/modules/dreamview/backend/simulator/scenario/parser.cc new file mode 100644 index 00000000..423e2a06 --- /dev/null +++ b/modules/dreamview/backend/simulator/scenario/parser.cc @@ -0,0 +1,92 @@ +#include "modules/dreamview/backend/simulator/scenario/parser.h" + +#include + +namespace apollo { +namespace dreamview { + +bool ScenarioParser::FromJson(const nlohmann::json& j, Scenario* out) { + if (!out) return false; + + auto convert_to_double = [](const nlohmann::json& val) -> double { + if (val.is_number()) return val.get(); + if (val.is_string()) return std::stod(val.get()); + return 0.0; + }; + + out->ego_waypoints.clear(); + out->static_obstacles.clear(); + out->dynamic_obstacles.clear(); + out->traffic_lights.clear(); + + // Ego waypoints from "p" array (like previous format) + if (j.contains("p")) { + const auto& p_array = j["p"]; + if (p_array.is_array() && p_array.size() >= 4) { + // create waypoints from pairs + for (size_t i = 0; i + 1 < p_array.size(); i += 2) { + out->ego_waypoints.emplace_back(convert_to_double(p_array[i]), + convert_to_double(p_array[i + 1])); + } + } + } + + // Static obstacles under "s" + if (j.contains("s")) { + for (const auto& item : j["s"]) { + if (!item.contains("p") || item["p"].size() < 2) continue; + Scenario::StaticObstacle so; + if (item.contains("id")) + so.id = static_cast(convert_to_double(item["id"])); + so.x = convert_to_double(item["p"][0]); + so.y = convert_to_double(item["p"][1]); + so.heading = item.contains("r") ? convert_to_double(item["r"]) : 0.0; + so.width = item.contains("w") ? convert_to_double(item["w"]) : 2.0; + so.length = item.contains("h") ? convert_to_double(item["h"]) : 5.0; + out->static_obstacles.push_back(so); + } + } + + // Dynamic obstacles under "d". Expect positions as Cartesian (x,y) and + // velocities vx,vy + if (j.contains("d")) { + for (const auto& item : j["d"]) { + if (!item.contains("p") || item["p"].size() < 2) continue; + if (!item.contains("v") || item["v"].size() < 2) continue; + Scenario::DynamicObstacle dobj; + if (item.contains("id")) + dobj.id = static_cast(convert_to_double(item["id"])); + dobj.x = convert_to_double(item["p"][0]); + dobj.y = convert_to_double(item["p"][1]); + dobj.vx = convert_to_double(item["v"][0]); + dobj.vy = convert_to_double(item["v"][1]); + dobj.width = item.contains("w") ? convert_to_double(item["w"]) : 2.0; + dobj.length = item.contains("h") ? convert_to_double(item["h"]) : 5.0; + dobj.trigger_time = + item.contains("t") ? convert_to_double(item["t"]) : 0.0; + out->dynamic_obstacles.push_back(dobj); + } + } + + // Traffic lights under "t" or "traffic_lights" + if (j.contains("t")) { + for (const auto& item : j["t"]) { + if (!item.contains("p") || item["p"].size() < 2) continue; + Scenario::TrafficLight tl; + if (item.contains("id")) + tl.id = static_cast(convert_to_double(item["id"])); + tl.x = convert_to_double(item["p"][0]); + tl.y = convert_to_double(item["p"][1]); + tl.state = item.contains("s") + ? static_cast(convert_to_double(item["s"])) + : 0; + tl.trigger_time = item.contains("t") ? convert_to_double(item["t"]) : 0.0; + out->traffic_lights.push_back(tl); + } + } + + return true; +} + +} // namespace dreamview +} // namespace apollo diff --git a/modules/dreamview/backend/simulator/scenario/parser.h b/modules/dreamview/backend/simulator/scenario/parser.h new file mode 100644 index 00000000..2c610c6a --- /dev/null +++ b/modules/dreamview/backend/simulator/scenario/parser.h @@ -0,0 +1,17 @@ +#pragma once + +#include "nlohmann/json.hpp" + +#include "modules/dreamview/backend/simulator/scenario/scenario.h" + +namespace apollo { +namespace dreamview { + +class ScenarioParser { + public: + // Parse JSON into Scenario. Returns true on success. + static bool FromJson(const nlohmann::json& j, Scenario* out); +}; + +} // namespace dreamview +} // namespace apollo diff --git a/modules/dreamview/backend/simulator/scenario/scenario.h b/modules/dreamview/backend/simulator/scenario/scenario.h new file mode 100644 index 00000000..4ca8e3e0 --- /dev/null +++ b/modules/dreamview/backend/simulator/scenario/scenario.h @@ -0,0 +1,43 @@ +#pragma once + +#include +#include + +#include "modules/common/math/vec2d.h" + +namespace apollo { +namespace dreamview { + +struct EgoConfig { + std::string type = "PERFECT_CONTROL"; // 模型类型 + double x = 0.0; + double y = 0.0; + double heading = 0.0; + double v = 0.0; + std::vector waypoints; +}; + +struct ObstacleConfig { + int id; + std::string type; // STATIC, DYNAMIC + double x, y, heading, v; + double length, width; + double trigger_time = 0.0; +}; + +struct MetricConfig { + std::string name; + double value = 0.0; // 例如最大仿真时间限制 +}; + +// 完整的场景蓝图 +struct Scenario { + std::string name; + EgoConfig ego_config; + std::vector obstacles; + // 可以继续扩展红绿灯、天气、路面系数等 + std::vector metrics; +}; + +} // namespace dreamview +} // namespace apollo diff --git a/modules/dreamview/backend/simulator/simulation.cc b/modules/dreamview/backend/simulator/simulation.cc new file mode 100644 index 00000000..cef56dce --- /dev/null +++ b/modules/dreamview/backend/simulator/simulation.cc @@ -0,0 +1,165 @@ +#include "modules/dreamview/backend/simulator/simulation.h" + +#include "modules/dreamview/backend/simulator/scenario/parser.h" +#include "modules/dreamview/backend/simulator/simulation_manager.h" + +namespace apollo { +namespace dreamview { + +Simulation::Simulation(MapService* map_service) : map_service_(map_service) { + engine_ = std::make_unique(map_service_); +} + +Simulation::~Simulation() { Stop(); } + +bool Simulation::Init(bool set_start_point, double start_velocity, + double start_acceleration, double start_heading) { + (void)set_start_point; + (void)start_velocity; + (void)start_acceleration; + (void)start_heading; + if (engine_) { + engine_->Init(); + scenario_manager_ = + std::make_unique(engine_->GetNode(), map_service_); + } + state_ = SimulationState::kIdle; + return engine_ != nullptr && scenario_manager_ != nullptr; +} + +void Simulation::Start() { + if (state_ == State::kIdle || state_ == State::kPaused) { + engine_->Start(); + state_ = State::kRunning; + } +} + +void Simulation::Stop() { + engine_->Stop(); + state_ = State::kPaused; +} + +void Simulation::Restart() { + Stop(); + // 1. 让 ScenarioManager 重新格式化初始状态 + // 2. 让 Ego 瞬移到 (x, y) + auto ego = scenario_manager_->GetEgo(); + if (ego) { + ego->Teleport(x, y, 0.0); // 默认 Heading 0 + ego->Reset(); // 清除旧的 Planning 轨迹 + } + // 3. 重置评估器 + engine_->GetEvaluator()->Reset(); + Start(); +} + +nlohmann::json Simulation::LoadDynamicModels() { return nlohmann::json(); } + +bool Simulation::AddDynamicModel(const std::string& dynamic_model_name) { + return true; +} + +bool Simulation::ChangeDynamicModel(const std::string& dynamic_model_name) { + return true; +} + +bool Simulation::DeleteDynamicModel(const std::string& dynamic_model_name) { + return true; +} + +void Simulation::ResetDynamicModel() {} + +bool Simulation::PlayScenario(const std::string& scenario_path) { + if (!engine_ || !scenario_manager_) { + state_ = SimulationState::kFault; + return false; + } + + state_ = SimulationState::kLoading; + + engine_->Stop(); + engine_->ClearEntities(); + + if (scenario_manager_->LoadScenario(scenario_path)) { + engine_->AddEntity(scenario_manager_->GetEgo()); + for (const auto& entity : scenario_manager_->GetEntities()) { + engine_->AddEntity(entity); + } + + auto evaluation_manager = std::make_shared(); + evaluation_manager->Init(); + engine_->AddEntity(evaluation_manager); + + engine_->Play(); + state_ = SimulationState::kRunning; + return true; + } else { + state_ = SimulationState::kFault; + return false; + } +} + +bool Simulation::PlayScenarioData(const nlohmann::json& scenario_data) { + if (!engine_ || !scenario_manager_) { + state_ = SimulationState::kFault; + return false; + } + + Scenario scenario; + if (!ScenarioParser::FromJson(scenario_data, &scenario)) { + state_ = SimulationState::kFault; + return false; + } + + state_ = SimulationState::kLoading; + engine_->Stop(); + engine_->ClearEntities(); + + if (!scenario_manager_->LoadScenarioData(scenario)) { + state_ = SimulationState::kFault; + return false; + } + + engine_->AddEntity(scenario_manager_->GetEgo()); + for (const auto& entity : scenario_manager_->GetEntities()) { + engine_->AddEntity(entity); + } + + auto evaluation_manager = std::make_shared(); + evaluation_manager->Init(); + engine_->AddEntity(evaluation_manager); + + engine_->Play(); + state_ = SimulationState::kRunning; + return true; +} + +void Simulation::ResetScenario() { + if (engine_) { + engine_->Reset(); + } +} + +uint64_t Simulation::GetTickCount() const { + if (!engine_) { + return 0; + } + return engine_->GetTickCount(); +} + +double Simulation::GetSimTimeSec() const { + if (!engine_) { + return 0.0; + } + return engine_->GetSimTimeSec(); +} + +bool Simulation::IsRunning() const { + if (!engine_) { + return false; + } + return engine_->IsRunning(); +} + +} // namespace dreamview +} // namespace apollo diff --git a/modules/dreamview/backend/simulator/simulation.h b/modules/dreamview/backend/simulator/simulation.h new file mode 100644 index 00000000..2c2591a1 --- /dev/null +++ b/modules/dreamview/backend/simulator/simulation.h @@ -0,0 +1,52 @@ +#pragma once + +#include +#include +#include +#include + +#include "nlohmann/json.hpp" + +#include "modules/dreamview/backend/map/map_service.h" +#include "modules/dreamview/backend/simulator/core/engine.h" +#include "modules/dreamview/backend/simulator/scenario/manager.h" + +namespace apollo { +namespace dreamview { + +class Simulation { + public: + explicit Simulation(MapService* map_service = nullptr); + virtual ~Simulation(); + + bool Init(bool set_start_point, double start_velocity = 0.0, + double start_acceleration = 0.0, + double start_heading = std::numeric_limits::max()); + + void Start(); + void Stop(); + void Restart(); + + enum class SimulationState { kIdle, kLoading, kRunning, kPaused, kFault }; + + bool IsEnabled() const { return state_ == SimulationState::kRunning; } + SimulationState GetState() const { return state_; } + + // Scenario operations + bool PlayScenario(const std::string& scenario_path); + bool PlayScenario(const nlohmann::json& scenario_data); + void ResetScenario(); + + uint64_t GetTickCount() const; + double GetSimTimeSec() const; + bool IsRunning() const; + + private: + std::unique_ptr engine_; + std::unique_ptr scenario_manager_; + MapService* map_service_; + SimulationState state_ = SimulationState::kIdle; +}; + +} // namespace dreamview +} // namespace apollo diff --git a/modules/dreamview/backend/simulator/simulator_main.cc b/modules/dreamview/backend/simulator/simulator_main.cc new file mode 100644 index 00000000..d24114dc --- /dev/null +++ b/modules/dreamview/backend/simulator/simulator_main.cc @@ -0,0 +1,129 @@ +#include +#include +#include + +#include "gflags/gflags.h" +#include "nlohmann/json.hpp" + +#include "cyber/cyber.h" +#include "modules/dreamview/backend/map/map_service.h" +#include "modules/dreamview/backend/simulator/simulation_manager.h" + +DEFINE_string(scenario_file, "", "Path to the scenario JSON file to play."); +DEFINE_int32(timeout_s, 60, + "Wall-clock timeout in seconds for a single scenario run."); +DEFINE_int32( + max_steps, 0, + "Maximum simulation ticks before graceful stop (0 means no step limit)."); +DEFINE_int32(poll_interval_ms, 10, + "Polling interval in milliseconds for run-state checks."); +DEFINE_string(result_json, "", + "Optional output path for machine-readable run result JSON."); + +namespace { + +void WriteResultJson(const std::string& path, const nlohmann::json& result) { + if (path.empty()) { + return; + } + std::ofstream ofs(path); + if (!ofs.is_open()) { + AERROR << "Failed to open result JSON path: " << path; + return; + } + ofs << result.dump(2); +} + +} // namespace + +int main(int argc, char* argv[]) { + google::ParseCommandLineFlags(&argc, &argv, true); + + // 初始化 CyberRT 环境 + apollo::cyber::Init(argv[0]); + AINFO << "Apollo Simulator Engine is starting..."; + + if (FLAGS_scenario_file.empty()) { + AERROR << "Please provide a scenario file using " + "--scenario_file=/path/to/scenario.json"; + return -1; + } + + // 1. 初始化依赖组件 (MapService) + // 参数为 false 表示不加载 routing topology (简化加载) + apollo::dreamview::MapService map_service(false); + + // 2. 构造并初始化仿真管家 + apollo::dreamview::SimulationManager sim_manager(&map_service); + if (!sim_manager.Init(true)) { + AERROR << "Failed to initialize Simulator Manager."; + return -1; + } + + // 3. 开始加载并运行场景 + AINFO << "Loading and Playing Scenario: " << FLAGS_scenario_file; + if (!sim_manager.PlayScenario(FLAGS_scenario_file)) { + AERROR << "Failed to load or play scenario: " << FLAGS_scenario_file; + nlohmann::json result = { + {"scenario_file", FLAGS_scenario_file}, + {"status", "fault"}, + {"reason", "play_scenario_failed"}, + {"tick_count", 0}, + {"sim_time_sec", 0.0}, + }; + WriteResultJson(FLAGS_result_json, result); + apollo::cyber::Clear(); + return 2; + } + + // 4. 单次任务运行(P1):满足 timeout/max_steps 即停止并退出 + const auto wall_start = std::chrono::steady_clock::now(); + std::string stop_reason = "max_steps_reached"; + bool success = true; + + while (apollo::cyber::OK() && sim_manager.IsRunning()) { + const auto now = std::chrono::steady_clock::now(); + const auto wall_elapsed = + std::chrono::duration_cast>(now - + wall_start) + .count(); + + if (FLAGS_timeout_s > 0 && wall_elapsed >= FLAGS_timeout_s) { + stop_reason = "timeout"; + success = false; + break; + } + + if (FLAGS_max_steps > 0 && + sim_manager.GetTickCount() >= static_cast(FLAGS_max_steps)) { + stop_reason = "max_steps_reached"; + success = true; + break; + } + + std::this_thread::sleep_for( + std::chrono::milliseconds(FLAGS_poll_interval_ms)); + } + + sim_manager.Stop(); + + const auto wall_end = std::chrono::steady_clock::now(); + const double wall_time_sec = + std::chrono::duration_cast>(wall_end - + wall_start) + .count(); + + nlohmann::json result = { + {"scenario_file", FLAGS_scenario_file}, + {"status", success ? "ok" : "timeout"}, + {"reason", stop_reason}, + {"tick_count", sim_manager.GetTickCount()}, + {"sim_time_sec", sim_manager.GetSimTimeSec()}, + {"wall_time_sec", wall_time_sec}, + }; + WriteResultJson(FLAGS_result_json, result); + + AINFO << "Apollo Simulator Engine shuts down."; + apollo::cyber::Clear(); + return success ? 0 : 3; +}