refactor sim_control interface (#3923)

modules/dreamview/backend/sim_control/sim_control.cc

@@ -18,6 +18,7 @@
 
 #include <cmath>
 
+#include "modules/common/math/linear_interpolation.h"
 #include "modules/common/math/math_utils.h"
 #include "modules/common/math/quaternion.h"
 #include "modules/common/time/time.h"
@@ -52,7 +53,7 @@ void TransformToVRF(const Point3D& point_mrf, const Quaternion& orientation,
   point_vrf->set_z(v_vrf.z());
 }
 
-bool CompareHeader(const Header& lhs, const Header& rhs) {
+bool IsSameHeader(const Header& lhs, const Header& rhs) {
   return lhs.sequence_num() == rhs.sequence_num() &&
          lhs.timestamp_sec() == rhs.timestamp_sec();
 }
@@ -60,14 +61,7 @@ bool CompareHeader(const Header& lhs, const Header& rhs) {
 }  // namespace
 
 SimControl::SimControl(const MapService* map_service)
-    : map_service_(map_service),
-      prev_point_index_(0),
-      next_point_index_(0),
-      received_planning_(false),
-      planning_count_(-1),
-      re_routing_triggered_(false),
-      enabled_(false),
-      inited_(false) {}
+    : map_service_(map_service) {}
 
 void SimControl::Init(bool set_start_point, double start_velocity,
                       double start_acceleration) {
@@ -88,7 +82,13 @@ void SimControl::Init(bool set_start_point, double start_velocity,
       AWARN << "Failed to get a dummy start point from map!";
       return;
     }
-    SetStartPoint(start_point.x(), start_point.y());
+    TrajectoryPoint point;
+    point.mutable_path_point()->set_x(start_point.x());
+    point.mutable_path_point()->set_y(start_point.y());
+    point.mutable_path_point()->set_z(start_point.z());
+    point.set_v(start_velocity);
+    point.set_a(start_acceleration);
+    SetStartPoint(point);
   }
 
   start_velocity_ = start_velocity;
@@ -106,25 +106,8 @@ void SimControl::OnReceiveNavigationInfo(
   }
 }
 
-void SimControl::SetStartPoint(const double x, const double y) {
-  next_point_.set_v(start_velocity_);
-  next_point_.set_a(start_acceleration_);
-
-  auto* next_point = next_point_.mutable_path_point();
-  next_point->set_x(x);
-  next_point->set_y(y);
-  next_point->set_z(0.0);
-
-  double theta = 0.0;
-  double s = 0.0;
-  if (!map_service_->GetPoseWithRegardToLane(next_point->x(), next_point->y(),
-                                             &theta, &s)) {
-    AERROR << "Failed to get heading from map! Treat theta and s as 0.0!";
-  }
-  next_point->set_theta(theta);
-  next_point->set_s(s);
-  next_point->set_kappa(0.0);
-
+void SimControl::SetStartPoint(const TrajectoryPoint& start_point) {
+  next_point_ = start_point;
   prev_point_index_ = next_point_index_ = 0;
   received_planning_ = false;
 }
@@ -148,7 +131,8 @@ void SimControl::OnRoutingResponse(const RoutingResponse& routing) {
     return;
   }
 
-  CHECK_LE(2, routing.routing_request().waypoint_size());
+  CHECK_GE(routing.routing_request().waypoint_size(), 2)
+      << "routing should have at least two waypoints";
   const auto& start_pose = routing.routing_request().waypoint(0).pose();
 
   current_routing_header_ = routing.header();
@@ -158,7 +142,17 @@ void SimControl::OnRoutingResponse(const RoutingResponse& routing) {
       routing.routing_request().header().module_name() == "planning";
   if (!re_routing_triggered_) {
     ClearPlanning();
-    SetStartPoint(start_pose.x(), start_pose.y());
+    TrajectoryPoint point;
+    point.mutable_path_point()->set_x(start_pose.x());
+    point.mutable_path_point()->set_y(start_pose.y());
+    point.set_a(0.0);
+    point.set_v(0.0);
+    double theta = 0.0;
+    double s = 0.0;
+    map_service_->GetPoseWithRegardToLane(start_pose.x(), start_pose.y(),
+                                          &theta, &s);
+    point.mutable_path_point()->set_theta(theta);
+    SetStartPoint(point);
   }
 }
 
@@ -189,7 +183,7 @@ void SimControl::OnPlanning(const apollo::planning::ADCTrajectory& trajectory) {
   // Reset current trajectory and the indices upon receiving a new trajectory.
   // The routing SimControl owns must match with the one Planning has.
   if (re_routing_triggered_ ||
-      CompareHeader(trajectory.routing_header(), current_routing_header_)) {
+      IsSameHeader(trajectory.routing_header(), current_routing_header_)) {
     // Hold a few cycles until the position information is fully refreshed on
     // planning side. Don't wait for the very first planning received.
     ++planning_count_;
@@ -211,70 +205,60 @@ void SimControl::Freeze() {
   prev_point_ = next_point_;
 }
 
-double SimControl::AbsoluteTimeOfNextPoint() {
-  return current_trajectory_.header().timestamp_sec() +
-         current_trajectory_.trajectory_point(next_point_index_)
-             .relative_time();
-}
-
-bool SimControl::NextPointWithinRange() {
-  return next_point_index_ < current_trajectory_.trajectory_point_size() - 1;
-}
+void SimControl::TimerCallback(const ros::TimerEvent& event) { RunOnce(); }
 
-void SimControl::TimerCallback(const ros::TimerEvent& event) {
-  RunOnce();
+void SimControl::RunOnce() {
+  TrajectoryPoint trajectory_point;
+  if (!PerfectControlModel(&trajectory_point)) {
+    AERROR << "Failed to calculate next point with perfect control model";
+    return;
+  }
+  PublishChassis(trajectory_point.v());
+  PublishLocalization(trajectory_point);
 }
 
-void SimControl::RunOnce() {
+bool SimControl::PerfectControlModel(TrajectoryPoint* point) {
   // Result of the interpolation.
-  double lambda = 0.0;
-  auto current_time = Clock::NowInSeconds();
+  auto relative_time =
+      Clock::NowInSeconds() - current_trajectory_.header().timestamp_sec();
+  const auto& trajectory = current_trajectory_.trajectory_point();
 
   if (!received_planning_) {
     prev_point_ = next_point_;
   } else {
-    if (current_trajectory_.estop().is_estop() || !NextPointWithinRange()) {
+    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.
-      double timestamp = current_trajectory_.header().timestamp_sec();
-
-      while (NextPointWithinRange() &&
-             current_time > AbsoluteTimeOfNextPoint()) {
+      while (next_point_index_ < trajectory.size() &&
+             relative_time >
+                 trajectory.Get(next_point_index_).relative_time()) {
         ++next_point_index_;
       }
 
       if (next_point_index_ == 0) {
         AERROR << "First trajectory point is a future point!";
-        return;
+        return false;
       }
 
-      if (current_time > AbsoluteTimeOfNextPoint()) {
-        prev_point_index_ = next_point_index_;
-      } else {
-        prev_point_index_ = next_point_index_ - 1;
+      if (next_point_index_ >= trajectory.size()) {
+        next_point_index_ = trajectory.size() - 1;
       }
+      prev_point_index_ = next_point_index_ - 1;
 
-      next_point_ = current_trajectory_.trajectory_point(next_point_index_);
-      prev_point_ = current_trajectory_.trajectory_point(prev_point_index_);
-
-      // Calculate the ratio based on the position of current time in
-      // between the previous point and the next point, where lambda =
-      // (current_point - prev_point) / (next_point - prev_point).
-      if (next_point_index_ != prev_point_index_) {
-        lambda = (current_time - timestamp - prev_point_.relative_time()) /
-                 (next_point_.relative_time() - prev_point_.relative_time());
-      }
+      next_point_ = trajectory.Get(next_point_index_);
+      prev_point_ = trajectory.Get(prev_point_index_);
     }
   }
-
-  PublishChassis(lambda);
-  PublishLocalization(lambda);
+  *point = apollo::common::math::InterpolateUsingLinearApproximation(
+      prev_point_, next_point_, relative_time);
+  return true;
 }
 
-void SimControl::PublishChassis(double lambda) {
+void SimControl::PublishChassis(double cur_speed) {
   Chassis chassis;
   AdapterManager::FillChassisHeader("SimControl", &chassis);
 
@@ -282,7 +266,6 @@ void SimControl::PublishChassis(double lambda) {
   chassis.set_driving_mode(Chassis::COMPLETE_AUTO_DRIVE);
   chassis.set_gear_location(Chassis::GEAR_DRIVE);
 
-  double cur_speed = Interpolate(prev_point_.v(), next_point_.v(), lambda);
   chassis.set_speed_mps(cur_speed);
   chassis.set_throttle_percentage(0.0);
   chassis.set_brake_percentage(0.0);
@@ -290,7 +273,7 @@ void SimControl::PublishChassis(double lambda) {
   AdapterManager::PublishChassis(chassis);
 }
 
-void SimControl::PublishLocalization(double lambda) {
+void SimControl::PublishLocalization(const TrajectoryPoint& point) {
   LocalizationEstimate localization;
   AdapterManager::FillLocalizationHeader("SimControl", &localization);
 
@@ -299,19 +282,15 @@ void SimControl::PublishLocalization(double lambda) {
   auto next = next_point_.path_point();
 
   // Set position
-  double cur_x = Interpolate(prev.x(), next.x(), lambda);
-  pose->mutable_position()->set_x(cur_x);
-  double cur_y = Interpolate(prev.y(), next.y(), lambda);
-  pose->mutable_position()->set_y(cur_y);
-  double cur_z = Interpolate(prev.z(), next.z(), lambda);
-  pose->mutable_position()->set_z(cur_z);
+  pose->mutable_position()->set_x(point.path_point().x());
+  pose->mutable_position()->set_y(point.path_point().y());
+  pose->mutable_position()->set_z(point.path_point().z());
   // Set orientation and heading
-  double cur_theta = NormalizeAngle(
-      prev.theta() + lambda * NormalizeAngle(next.theta() - prev.theta()));
+  double cur_theta = point.path_point().theta();
 
   if (FLAGS_use_navigation_mode) {
-    double flu_x = cur_x;
-    double flu_y = cur_y;
+    double flu_x = point.path_point().x();
+    double flu_y = point.path_point().y();
     double enu_x = 0.0;
     double enu_y = 0.0;
     common::math::RotateAxis(-cur_theta, flu_x, flu_y, &enu_x, &enu_y);
@@ -330,28 +309,25 @@ void SimControl::PublishLocalization(double lambda) {
   pose->set_heading(cur_theta);
 
   // Set linear_velocity
-  double cur_speed = Interpolate(prev_point_.v(), next_point_.v(), lambda);
-  pose->mutable_linear_velocity()->set_x(std::cos(cur_theta) * cur_speed);
-  pose->mutable_linear_velocity()->set_y(std::sin(cur_theta) * cur_speed);
+  pose->mutable_linear_velocity()->set_x(std::cos(cur_theta) * point.v());
+  pose->mutable_linear_velocity()->set_y(std::sin(cur_theta) * point.v());
   pose->mutable_linear_velocity()->set_z(0);
 
   // Set angular_velocity in both map reference frame and vehicle reference
   // frame
-  double cur_curvature = Interpolate(prev.kappa(), next.kappa(), lambda);
   pose->mutable_angular_velocity()->set_x(0);
   pose->mutable_angular_velocity()->set_y(0);
-  pose->mutable_angular_velocity()->set_z(cur_speed * cur_curvature);
+  pose->mutable_angular_velocity()->set_z(point.v() *
+                                          point.path_point().kappa());
 
   TransformToVRF(pose->angular_velocity(), pose->orientation(),
                  pose->mutable_angular_velocity_vrf());
 
   // Set linear_acceleration in both map reference frame and vehicle reference
   // frame
-  double cur_acceleration_s =
-      Interpolate(prev_point_.a(), next_point_.a(), lambda);
   auto* linear_acceleration = pose->mutable_linear_acceleration();
-  linear_acceleration->set_x(std::cos(cur_theta) * cur_acceleration_s);
-  linear_acceleration->set_y(std::sin(cur_theta) * cur_acceleration_s);
+  linear_acceleration->set_x(std::cos(cur_theta) * point.a());
+  linear_acceleration->set_y(std::sin(cur_theta) * point.a());
   linear_acceleration->set_z(0);
 
   TransformToVRF(pose->linear_acceleration(), pose->orientation(),

modules/dreamview/backend/sim_control/sim_control.h

@@ -24,11 +24,13 @@
 #include <string>
 
 #include "gtest/gtest_prod.h"
+
+#include "modules/map/relative_map/proto/navigation.pb.h"
+
 #include "modules/common/adapters/adapter_manager.h"
 #include "modules/dreamview/backend/common/dreamview_gflags.h"
 #include "modules/dreamview/backend/map/map_service.h"
 #include "modules/dreamview/backend/sim_control/sim_control_interface.h"
-#include "modules/map/relative_map/proto/navigation.pb.h"
 
 /**
  * @namespace apollo::dreamview
@@ -51,9 +53,7 @@ class SimControl : SimControlInterface {
    */
   explicit SimControl(const MapService *map_service);
 
-  inline bool IsEnabled() const {
-    return enabled_;
-  }
+  bool IsEnabled() const { return enabled_; }
 
   /**
    * @brief setup callbacks and timer
@@ -78,9 +78,6 @@ class SimControl : SimControlInterface {
    */
   void Reset() override;
 
-  /**
-   * @brief Publish simulated localization and chassis.
-   */
   void RunOnce() override;
 
  private:
@@ -89,28 +86,26 @@ class SimControl : SimControlInterface {
   void OnReceiveNavigationInfo(
       const relative_map::NavigationInfo &navigation_info);
 
+  /**
+   * @brief Predict the next trajectory point using perfect control model
+   */
+  bool PerfectControlModel(apollo::common::TrajectoryPoint *point);
+
+  void PublishChassis(double cur_speed);
+
+  void PublishLocalization(const apollo::common::TrajectoryPoint &point);
+
   // Reset the start point, which can be a dummy point on the map or received
   // from the routing module.
-  void SetStartPoint(const double x, const double y);
+  void SetStartPoint(const apollo::common::TrajectoryPoint &point);
 
   void Freeze();
 
-  double AbsoluteTimeOfNextPoint();
-  bool NextPointWithinRange();
-
   void TimerCallback(const ros::TimerEvent &event);
 
-  void PublishChassis(double lambda);
-  void PublishLocalization(double lambda);
-
   void ClearPlanning();
 
-  template <typename T>
-  T Interpolate(T prev, T next, double lambda) {
-    return (1 - lambda) * prev + lambda * next;
-  }
-
-  const MapService *map_service_;
+  const MapService *map_service_ = nullptr;
 
   // The timer to publish simulated localization and chassis messages.
   ros::Timer sim_control_timer_;
@@ -122,20 +117,20 @@ class SimControl : SimControlInterface {
   apollo::planning::ADCTrajectory current_trajectory_;
   // The index of the previous and next point with regard to the
   // current_trajectory.
-  int prev_point_index_;
-  int next_point_index_;
+  int prev_point_index_ = 0;
+  int next_point_index_ = 0;
 
   // Whether there's a planning received after the most recent routing.
-  bool received_planning_;
+  bool received_planning_ = false;
 
   // Number of planning received in terms of one RoutingResponse.
-  int planning_count_;
+  int planning_count_ = -1;
 
-  bool re_routing_triggered_;
+  bool re_routing_triggered_ = false;
 
   // Whether the sim control is enabled / initialized.
-  bool enabled_;
-  bool inited_;
+  bool enabled_ = false;
+  bool inited_ = false;
 
   // The header of the routing planning is following.
   apollo::common::Header current_routing_header_;

modules/dreamview/backend/sim_control/sim_control_test.cc

@@ -16,6 +16,8 @@
 
 #include "modules/dreamview/backend/sim_control/sim_control.h"
 
+#include <cmath>
+
 #include "ros/include/ros/ros.h"
 
 #include "modules/common/adapters/proto/adapter_config.pb.h"
@@ -84,8 +86,7 @@ class SimControlTest : public ::testing::Test {
       auto *sub_config = config.add_config();
       sub_config->set_mode(
           apollo::common::adapter::AdapterConfig::RECEIVE_ONLY);
-      sub_config->set_type(
-          apollo::common::adapter::AdapterConfig::NAVIGATION);
+      sub_config->set_type(apollo::common::adapter::AdapterConfig::NAVIGATION);
     }
 
     AdapterManager::Init(config);
@@ -99,7 +100,8 @@ class SimControlTest : public ::testing::Test {
 };
 
 void SetTrajectory(const std::vector<double> &xs, const std::vector<double> &ys,
-                   const std::vector<double> &vs, const std::vector<double> &as,
+                   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) {
@@ -107,6 +109,7 @@ void SetTrajectory(const std::vector<double> &xs, const std::vector<double> &ys,
     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]);
@@ -117,19 +120,32 @@ void SetTrajectory(const std::vector<double> &xs, const std::vector<double> &ys,
 
 TEST_F(SimControlTest, Test) {
   planning::ADCTrajectory adc_trajectory;
-  std::vector<double> xs = {1.0, 1.1, 1.2, 1.3, 1.4};
-  std::vector<double> ys = {1.0, 1.1, 1.2, 1.3, 1.4};
-  std::vector<double> vs = {40.0, 50.0, 60.0, 70.0, 80.0};
-  std::vector<double> as = {40.0, 50.0, 60.0, 70.0, 80.0};
-  std::vector<double> ths = {0.2, 0.6, 0.8, 1.0, 1.5};
-  std::vector<double> ks = {1.0, 2.0, 3.0, 4.0, 5.0};
+  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};
-  SetTrajectory(xs, ys, vs, as, ths, ks, ts, &adc_trajectory);
+  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(1.0, 1.0);
+  sim_control_->SetStartPoint(adc_trajectory.trajectory_point(0));
   AdapterManager::PublishPlanning(adc_trajectory);
 
   {
@@ -146,16 +162,16 @@ TEST_F(SimControlTest, Test) {
     EXPECT_EQ(Chassis::COMPLETE_AUTO_DRIVE, chassis->driving_mode());
     EXPECT_EQ(Chassis::GEAR_DRIVE, chassis->gear_location());
 
-    EXPECT_NEAR(chassis->speed_mps(), 41.0, 1e-6);
+    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(), 1.01, 1e-6);
-    EXPECT_NEAR(pose.position().y(), 1.01, 1e-6);
+    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 = 0.24;
+    const double theta = M_PI / 4.0;
     EXPECT_NEAR(pose.heading(), theta, 1e-6);
 
     const Eigen::Quaternion<double> orientation =
@@ -165,17 +181,17 @@ TEST_F(SimControlTest, Test) {
     EXPECT_NEAR(pose.orientation().qy(), orientation.y(), 1e-6);
     EXPECT_NEAR(pose.orientation().qz(), orientation.z(), 1e-6);
 
-    const double speed = 41.0;
+    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 = 1.1;
+    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 = 41.0;
+    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(),

modules/tools/manual_traffic_light/manual_traffic_light.cc

@@ -39,19 +39,19 @@
 #include "modules/map/hdmap/hdmap_util.h"
 
 using apollo::common::PointENU;
+using apollo::common::adapter::AdapterConfig;
+using apollo::common::adapter::AdapterManager;
+using apollo::common::adapter::AdapterManagerConfig;
+using apollo::common::color::ANSI_GREEN;
+using apollo::common::color::ANSI_RED;
+using apollo::common::color::ANSI_RESET;
+using apollo::common::util::PrintIter;
 using apollo::hdmap::HDMap;
 using apollo::hdmap::HDMapUtil;
 using apollo::hdmap::SignalInfoConstPtr;
-using apollo::common::adapter::AdapterManager;
-using apollo::common::adapter::AdapterConfig;
-using apollo::common::adapter::AdapterManagerConfig;
 using apollo::localization::LocalizationEstimate;
 using apollo::perception::TrafficLight;
 using apollo::perception::TrafficLightDetection;
-using apollo::common::util::PrintIter;
-using apollo::common::color::ANSI_RED;
-using apollo::common::color::ANSI_GREEN;
-using apollo::common::color::ANSI_RESET;
 
 DEFINE_bool(all_lights, false, "set all lights on the map");
 
@@ -90,7 +90,7 @@ bool GetTrafficLightsWithinDistance(
   CHECK_NOTNULL(traffic_lights);
   AdapterManager::Observe();
   if (AdapterManager::GetLocalization()->Empty()) {
-    ADEBUG << "No localization received";
+    AERROR << "No localization received";
     return false;
   }
   const auto *hdmap = HDMapUtil::BaseMapPtr();