Dreamview: adding testcases

modules/dreamview/backend/simulation_world/simulation_world_service.h

@@ -244,6 +244,10 @@ class SimulationWorldService {
   FRIEND_TEST(SimulationWorldServiceTest, UpdateDecision);
   FRIEND_TEST(SimulationWorldServiceTest, UpdatePrediction);
   FRIEND_TEST(SimulationWorldServiceTest, UpdateRouting);
+  FRIEND_TEST(SimulationWorldServiceTest, UpdateGps);
+  FRIEND_TEST(SimulationWorldServiceTest, UpdateControlCommandWithSimpleLonLat);
+  FRIEND_TEST(SimulationWorldServiceTest, UpdateControlCommandWithSimpleMpc);
+  FRIEND_TEST(SimulationWorldServiceTest, DownsampleSpeedPointsByInterval);
 };
 
 }  // namespace dreamview

modules/dreamview/backend/simulation_world/simulation_world_service_test.cc

@@ -479,5 +479,103 @@ TEST_F(SimulationWorldServiceTest, UpdateRouting) {
   }
 }
 
+TEST_F(SimulationWorldServiceTest, UpdateGps) {
+  // Prepare the gps message that will be used to update the
+  // SimulationWorld object.
+  apollo::localization::Gps gps;
+  auto* pose = gps.mutable_localization();
+  auto* position = pose->mutable_position();
+  position->set_x(586922.10396);
+  position->set_y(4141065.07993);
+  position->set_z(-30.60015);
+  auto* orientation = pose->mutable_orientation();
+  orientation->set_qx(-0.01742);
+  orientation->set_qy(0.00928);
+  orientation->set_qz(-0.12262);
+  orientation->set_qw(0.99226);
+
+  sim_world_service_->UpdateSimulationWorld(gps);
+
+  // Verify
+  const auto& gps_position = sim_world_service_->world().gps();
+  EXPECT_DOUBLE_EQ(586922.10396, gps_position.position_x());
+  EXPECT_DOUBLE_EQ(4141065.07993, gps_position.position_y());
+  EXPECT_NEAR(1.32515, gps_position.heading(), 0.00001);
+}
+
+TEST_F(SimulationWorldServiceTest, UpdateControlCommandWithSimpleLonLat) {
+  // Prepare the ControlCommand message that will be used to update the
+  // SimulationWorld object.
+  apollo::control::ControlCommand control_command;
+  const double station_error = 0.90225;
+  const double heading_error = -0.00097;
+  const double lateral_error = 0.004176;
+  control_command.mutable_header()->set_timestamp_sec(2000);
+  auto* debug = control_command.mutable_debug();
+  auto* simple_lon = debug->mutable_simple_lon_debug();
+  simple_lon->set_station_error(station_error);
+
+  auto* simple_lat = debug->mutable_simple_lat_debug();
+  simple_lat->set_heading_error(heading_error);
+  simple_lat->set_lateral_error(lateral_error);
+  sim_world_service_->UpdateSimulationWorld(control_command);
+
+  // Verify
+  const auto& control_data = sim_world_service_->world().control_data();
+  EXPECT_DOUBLE_EQ(station_error, control_data.station_error());
+  EXPECT_DOUBLE_EQ(heading_error, control_data.heading_error());
+  EXPECT_DOUBLE_EQ(lateral_error, control_data.lateral_error());
+}
+
+TEST_F(SimulationWorldServiceTest, UpdateControlCommandWithSimpleMpc) {
+  // Prepare the ControlCommand message that will be used to update the
+  // SimulationWorld object.
+  apollo::control::ControlCommand control_command;
+  const double station_error = 0.91225;
+  const double heading_error = -0.01097;
+  const double lateral_error = 0.014176;
+  control_command.mutable_header()->set_timestamp_sec(3000);
+  auto* debug = control_command.mutable_debug();
+  auto* simple_mpc = debug->mutable_simple_mpc_debug();
+  simple_mpc->set_station_error(station_error);
+  simple_mpc->set_heading_error(heading_error);
+  simple_mpc->set_lateral_error(lateral_error);
+  sim_world_service_->UpdateSimulationWorld(control_command);
+
+  // Verify
+  const auto& control_data = sim_world_service_->world().control_data();
+  EXPECT_DOUBLE_EQ(station_error, control_data.station_error());
+  EXPECT_DOUBLE_EQ(heading_error, control_data.heading_error());
+  EXPECT_DOUBLE_EQ(lateral_error, control_data.lateral_error());
+}
+
+TEST_F(SimulationWorldServiceTest, DownsampleSpeedPointsByInterval) {
+  apollo::planning_internal::STGraphDebug graph;
+  auto* speed_points = graph.mutable_speed_profile();
+  for (int i = 0; i < 10; ++i) {
+    auto* point = speed_points->Add();
+    point->set_s(i * 1.1);
+    point->set_t(i * 1.2);
+    point->set_v(i * 1.3);
+  }
+
+  size_t interval = 5;
+  auto* downsampled_points = sim_world_service_->world_.mutable_planning_data()
+                                 ->add_st_graph()
+                                 ->mutable_speed_profile();
+  sim_world_service_->DownsampleSpeedPointsByInterval(*speed_points, interval,
+                                                      downsampled_points);
+
+  // Verify
+  std::vector<int> kept_index = {0, 5, 9};
+  EXPECT_EQ(kept_index.size(), downsampled_points->size());
+  for (int i = 0; i < 3; ++i) {
+    auto& point = downsampled_points->Get(i);
+    EXPECT_DOUBLE_EQ(kept_index[i] * 1.1, point.s());
+    EXPECT_DOUBLE_EQ(kept_index[i] * 1.2, point.t());
+    EXPECT_DOUBLE_EQ(kept_index[i] * 1.3, point.v());
+  }
+}
+
 }  // namespace dreamview
 }  // namespace apollo