change apollo change3.

src/apollo/change3/sim_control_test.cc

@@ -1,215 +0,0 @@
-/******************************************************************************
- * Copyright 2017 The Apollo Authors. All Rights Reserved.
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- * http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- *****************************************************************************/
-
-#include "modules/dreamview/backend/common/sim_control_manager/dynamic_model/perfect_control/sim_perfect_control.h"
-
-#include "cyber/blocker/blocker_manager.h"
-#include "cyber/time/clock.h"
-
-#include "gmock/gmock.h"
-#include "gtest/gtest.h"
-
-#include "modules/common_msgs/chassis_msgs/chassis.pb.h"
-#include "modules/common/adapters/adapter_gflags.h"
-#include "modules/common/math/quaternion.h"
-
-using apollo::canbus::Chassis;
-using apollo::common::math::HeadingToQuaternion;
-using apollo::cyber::Clock;
-using apollo::cyber::ClockMode;
-using apollo::cyber::blocker::BlockerManager;
-using apollo::localization::LocalizationEstimate;
-using apollo::planning::ADCTrajectory;
-using apollo::prediction::PredictionObstacles;
-
-namespace apollo {
-namespace dreamview {
-
-class SimControlTest : public ::testing::Test {
- public:
-  static void SetUpTestCase() {
-    cyber::GlobalData::Instance()->EnableSimulationMode();
-  }
-
-  virtual void SetUp() {
-    FLAGS_map_dir = "modules/dreamview/backend/testdata";
-    FLAGS_base_map_filename = "garage.bin";
-
-    map_service_.reset(new MapService(false));
-    sim_control_.reset(new SimPerfectControl(map_service_.get()));
-  }
-
- protected:
-  std::unique_ptr<MapService> map_service_;
-  std::unique_ptr<SimPerfectControl> sim_control_;
-};
-
-void SetTrajectory(const std::vector<double> &xs, const std::vector<double> &ys,
-                   const std::vector<double> &ss, const std::vector<double> &vs,
-                   const std::vector<double> &as,
-                   const std::vector<double> &ths,
-                   const std::vector<double> &ks, const std::vector<double> &ts,
-                   planning::ADCTrajectory *adc_trajectory) {
-  for (size_t i = 0; i < xs.size(); ++i) {
-    auto *point = adc_trajectory->add_trajectory_point();
-    point->mutable_path_point()->set_x(xs[i]);
-    point->mutable_path_point()->set_y(ys[i]);
-    point->mutable_path_point()->set_s(ss[i]);
-    point->set_v(vs[i]);
-    point->set_a(as[i]);
-    point->mutable_path_point()->set_theta(ths[i]);
-    point->mutable_path_point()->set_kappa(ks[i]);
-    point->set_relative_time(ts[i]);
-  }
-  adc_trajectory->set_gear(Chassis::GEAR_DRIVE);
-}
-
-TEST_F(SimControlTest, Test) {
-  sim_control_->Init(false);
-  sim_control_->enabled_ = true;
-
-  planning::ADCTrajectory adc_trajectory;
-  std::vector<double> xs(5);
-  std::vector<double> ys(5);
-  std::vector<double> ss(5);
-  std::vector<double> vs(5);
-  std::vector<double> as = {0.0, 0.0, 0.0, 0.0, 0.0};
-  std::vector<double> ths = {M_PI / 4.0, M_PI / 4.0, M_PI / 4.0, M_PI / 4.0,
-                             M_PI / 4.0};
-  std::vector<double> kappa_s = {0.0, 0.0, 0.0, 0.0, 0.0};
-  std::vector<double> ts = {0.0, 0.1, 0.2, 0.3, 0.4};
-  ss[0] = 0.0;
-  xs[0] = 0.0;
-  ys[0] = 0.0;
-  vs[0] = 10.0;
-  for (std::size_t i = 1; i < ts.size(); ++i) {
-    vs[i] = vs[i - 1] + as[i - 1] * ts[i];
-    ss[i] = (vs[i - 1] + 0.5 * vs[i]) * ts[i];
-    xs[i] = std::sqrt(ss[i] * ss[i] / 2.0);
-    ys[i] = std::sqrt(ss[i] * ss[i] / 2.0);
-  }
-
-  SetTrajectory(xs, ys, ss, vs, as, ths, kappa_s, ts, &adc_trajectory);
-
-  const double timestamp = 100.0;
-  adc_trajectory.mutable_header()->set_timestamp_sec(timestamp);
-
-  sim_control_->SetStartPoint(adc_trajectory.trajectory_point(0));
-  sim_control_->OnPlanning(std::make_shared<ADCTrajectory>(adc_trajectory));
-
-  {
-    Clock::SetMode(ClockMode::MODE_MOCK);
-    Clock::SetNowInSeconds(100.01);
-    sim_control_->RunOnce();
-
-    BlockerManager::Instance()->Observe();
-    auto localization =
-        BlockerManager::Instance()
-            ->GetBlocker<LocalizationEstimate>(FLAGS_localization_topic)
-            ->GetLatestObservedPtr();
-    auto chassis = BlockerManager::Instance()
-                       ->GetBlocker<Chassis>(FLAGS_chassis_topic)
-                       ->GetLatestObservedPtr();
-
-    EXPECT_TRUE(chassis->engine_started());
-    EXPECT_EQ(Chassis::COMPLETE_AUTO_DRIVE, chassis->driving_mode());
-    EXPECT_EQ(Chassis::GEAR_DRIVE, chassis->gear_location());
-
-    EXPECT_NEAR(chassis->speed_mps(), 10.0, 1e-6);
-    EXPECT_NEAR(chassis->throttle_percentage(), 0.0, 1e-6);
-    EXPECT_NEAR(chassis->brake_percentage(), 0.0, 1e-6);
-
-    const auto &pose = localization->pose();
-    EXPECT_NEAR(pose.position().x(), 0.10606601717803638, 1e-6);
-    EXPECT_NEAR(pose.position().y(), 0.10606601717803638, 1e-6);
-    EXPECT_NEAR(pose.position().z(), 0.0, 1e-6);
-
-    const double theta = M_PI / 4.0;
-    EXPECT_NEAR(pose.heading(), theta, 1e-6);
-
-    const Eigen::Quaternion<double> orientation =
-        HeadingToQuaternion<double>(theta);
-    EXPECT_NEAR(pose.orientation().qw(), orientation.w(), 1e-6);
-    EXPECT_NEAR(pose.orientation().qx(), orientation.x(), 1e-6);
-    EXPECT_NEAR(pose.orientation().qy(), orientation.y(), 1e-6);
-    EXPECT_NEAR(pose.orientation().qz(), orientation.z(), 1e-6);
-
-    const double speed = 10.0;
-    EXPECT_NEAR(pose.linear_velocity().x(), std::cos(theta) * speed, 1e-6);
-    EXPECT_NEAR(pose.linear_velocity().y(), std::sin(theta) * speed, 1e-6);
-    EXPECT_NEAR(pose.linear_velocity().z(), 0.0, 1e-6);
-
-    const double curvature = 0.0;
-    EXPECT_NEAR(pose.angular_velocity().x(), 0.0, 1e-6);
-    EXPECT_NEAR(pose.angular_velocity().y(), 0.0, 1e-6);
-    EXPECT_NEAR(pose.angular_velocity().z(), speed * curvature, 1e-6);
-
-    const double acceleration_s = 0.0;
-    EXPECT_NEAR(pose.linear_acceleration().x(),
-                std::cos(theta) * acceleration_s, 1e-6);
-    EXPECT_NEAR(pose.linear_acceleration().y(),
-                std::sin(theta) * acceleration_s, 1e-6);
-    EXPECT_NEAR(pose.linear_acceleration().z(), 0.0, 1e-6);
-  }
-}
-
-TEST_F(SimControlTest, TestDummyPrediction) {
-  Clock::SetMode(ClockMode::MODE_MOCK);
-
-  sim_control_->Init(false);
-  sim_control_->enabled_ = true;
-
-  auto obstacles = std::make_shared<PredictionObstacles>();
-
-  {
-    const double timestamp = 100.01;
-    Clock::SetNowInSeconds(timestamp);
-    obstacles->mutable_header()->set_timestamp_sec(timestamp);
-    obstacles->mutable_header()->set_module_name("NoneSimPrediction");
-    sim_control_->OnPredictionObstacles(obstacles);
-
-    sim_control_->PublishDummyPrediction();
-
-    BlockerManager::Instance()->Observe();
-    EXPECT_FALSE(sim_control_->send_dummy_prediction_);
-    EXPECT_TRUE(BlockerManager::Instance()
-                    ->GetBlocker<PredictionObstacles>(FLAGS_prediction_topic)
-                    ->IsObservedEmpty());
-  }
-
-  sim_control_->InternalReset();
-
-  {
-    const double timestamp = 100.2;
-    Clock::SetNowInSeconds(timestamp);
-    obstacles->mutable_header()->set_timestamp_sec(timestamp);
-    obstacles->mutable_header()->set_module_name("SimPrediction");
-    sim_control_->OnPredictionObstacles(obstacles);
-
-    sim_control_->PublishDummyPrediction();
-
-    EXPECT_TRUE(sim_control_->send_dummy_prediction_);
-    BlockerManager::Instance()->Observe();
-    auto prediction =
-        BlockerManager::Instance()
-            ->GetBlocker<PredictionObstacles>(FLAGS_prediction_topic)
-            ->GetLatestObservedPtr();
-    EXPECT_EQ("SimPrediction", prediction->header().module_name());
-    EXPECT_DOUBLE_EQ(prediction->header().timestamp_sec(), timestamp);
-  }
-}
-}  // namespace dreamview
-}  // namespace apollo

src/apollo/change3/sim_perfect_control.h

@@ -0,0 +1,216 @@
+/******************************************************************************
+ * Copyright 2017 The Apollo Authors. All Rights Reserved.
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *****************************************************************************/
+
+/**
+ * @file
+ */
+
+#pragma once
+
+#include <memory>
+
+#include "gtest/gtest_prod.h"
+
+#include "modules/common_msgs/localization_msgs/localization.pb.h"
+#include "modules/common_msgs/planning_msgs/navigation.pb.h"
+#include "modules/common_msgs/planning_msgs/planning.pb.h"
+#include "modules/common_msgs/planning_msgs/planning_command.pb.h"
+#include "modules/common_msgs/prediction_msgs/prediction_obstacle.pb.h"
+#include "modules/common_msgs/routing_msgs/routing.pb.h"
+
+#include "cyber/cyber.h"
+#include "modules/dreamview/backend/common/dreamview_gflags.h"
+#include "modules/dreamview/backend/common/map_service/map_service.h"
+#include "modules/dreamview/backend/common/sim_control_manager/core/sim_control_base.h"
+
+/**
+ * @namespace apollo::dreamview
+ * @brief apollo::dreamview
+ */
+namespace apollo {
+namespace dreamview {
+
+/**
+ * @class SimPerfectControl
+ * @brief A module that simulates a 'perfect control' algorithm, which assumes
+ * an ideal world where the car can be perfectly placed wherever the planning
+ * asks it to be, with the expected speed, acceleration, etc.
+ */
+class SimPerfectControl final : public SimControlBase {
+ public:
+  /**
+   * @brief Constructor of SimPerfectControl.
+   * @param map_service the pointer of MapService.
+   */
+  explicit SimPerfectControl(const MapService *map_service);
+
+  /**
+   * @brief setup callbacks and timer
+   * @param set_start_point initialize localization.
+   */
+  void Init(bool set_start_point, nlohmann::json start_point_attr,
+            bool use_start_point_position = false) override;
+
+  /**
+   * @brief Starts the timer to publish simulated localization and chassis
+   * messages.
+   */
+  void Start() override;
+
+  /**
+   * @brief Stops the timer.
+   */
+  void Stop() override;
+
+  /**
+   * @brief Set vehicle position.
+   */
+  void ReSetPoinstion(double x, double y, double heading) override;
+
+  /**
+   * @brief Resets the internal state.
+   */
+  void Reset() override;
+
+  void RunOnce() override;
+
+ private:
+  void OnPlanning(
+      const std::shared_ptr<apollo::planning::ADCTrajectory> &trajectory);
+  void OnPlanningCommand(
+      const std::shared_ptr<apollo::planning::PlanningCommand>
+          &planning_command);
+  void OnReceiveNavigationInfo(
+      const std::shared_ptr<apollo::relative_map::NavigationInfo>
+          &navigation_info);
+  void OnPredictionObstacles(
+      const std::shared_ptr<apollo::prediction::PredictionObstacles>
+          &obstacles);
+
+  /**
+   * @brief Predict the next trajectory point using perfect control model
+   */
+  bool PerfectControlModel(
+      apollo::common::TrajectoryPoint *point,
+      apollo::canbus::Chassis::GearPosition *gear_position);
+
+  void PublishChassis(double cur_speed,
+                      apollo::canbus::Chassis::GearPosition gear_position);
+
+  void PublishLocalization(const apollo::common::TrajectoryPoint &point);
+
+  void PublishDummyPrediction();
+
+  void InitTimerAndIO();
+
+  /**
+   * @brief Starts the timer to publish simulated localization and chassis
+   * messages. Designated Start point for scenario
+   */
+  void Start(double x, double y, double v = 0.0, double a = 0.0) override;
+
+  void InitStartPoint(double start_velocity, double start_acceleration);
+
+  // use scenario start point to init start point under the simulation
+  // condition.
+  void InitStartPoint(double x, double y, double start_velocity,
+                      double start_acceleration);
+
+  // Reset the start point, which can be a dummy point on the map, a current
+  // localization pose, or a start position received from the routing module.
+  void SetStartPoint(const apollo::common::TrajectoryPoint &point);
+
+  void Freeze();
+
+  void ClearPlanning();
+
+  void InternalReset();
+
+  const MapService *map_service_ = nullptr;
+
+  std::unique_ptr<cyber::Node> node_;
+
+  std::shared_ptr<cyber::Reader<apollo::localization::LocalizationEstimate>>
+      localization_reader_;
+  std::shared_ptr<cyber::Reader<apollo::planning::ADCTrajectory>>
+      planning_reader_;
+  std::shared_ptr<cyber::Reader<apollo::relative_map::NavigationInfo>>
+      navigation_reader_;
+  std::shared_ptr<cyber::Reader<apollo::prediction::PredictionObstacles>>
+      prediction_reader_;
+  std::shared_ptr<cyber::Reader<apollo::planning::PlanningCommand>>
+      planning_command_reader_;
+  std::shared_ptr<cyber::Writer<apollo::localization::LocalizationEstimate>>
+      localization_writer_;
+  std::shared_ptr<cyber::Writer<apollo::canbus::Chassis>> chassis_writer_;
+  std::shared_ptr<cyber::Writer<apollo::prediction::PredictionObstacles>>
+      prediction_writer_;
+
+  // The timer to publish simulated localization and chassis messages.
+  std::unique_ptr<cyber::Timer> sim_control_timer_;
+
+  // The timer to publish dummy prediction
+  std::unique_ptr<cyber::Timer> sim_prediction_timer_;
+
+  // Time interval of the timer, in milliseconds.
+  static constexpr double kSimControlIntervalMs = 10;
+  static constexpr double kSimPredictionIntervalMs = 100;
+
+  // The latest received planning trajectory.
+  std::shared_ptr<apollo::planning::ADCTrajectory> current_trajectory_;
+  // The index of the previous and next point with regard to the
+  // current_trajectory.
+  int prev_point_index_ = 0;
+  int next_point_index_ = 0;
+
+  // Whether there's a planning received after the most recent routing.
+  bool received_planning_ = false;
+
+  // Whether start point is initialized from actual localization data
+  bool start_point_from_localization_ = false;
+
+  // Whether to send dummy predictions
+  bool send_dummy_prediction_ = true;
+  
+  //增加的消息，用于使用真实的定位数据
+  std::shared_ptr<apollo::localization::LocalizationEstimate> adcloc_;
+  bool receiveadcloc = false;
+  double fNowVel = 0.0;
+  std::shared_ptr<cyber::Reader<apollo::localization::LocalizationEstimate>>
+      localization_adc_reader_;
+  void onADCLoc(const std::shared_ptr<apollo::localization::LocalizationEstimate> &xloc);
+  //增加段结束
+  
+
+  // The header of the routing planning is following.
+  apollo::common::Header current_routing_header_;
+
+  apollo::common::TrajectoryPoint prev_point_;
+  apollo::common::TrajectoryPoint next_point_;
+
+  common::PathPoint adc_position_;
+
+  // Linearize reader/timer callbacks and external operations.
+  std::mutex mutex_;
+  // Locks related to timer.
+  std::mutex timer_mutex_;
+
+  FRIEND_TEST(SimControlTest, Test);
+  FRIEND_TEST(SimControlTest, TestDummyPrediction);
+};
+
+}  // namespace dreamview
+}  // namespace apollo