dreamview: add sim control initial support in navigation mode. still need more...

dreamview: add sim control initial support in navigation mode. still need more code to make it work.

modules/dreamview/backend/sim_control/BUILD

@@ -15,6 +15,7 @@ cc_library(
         "//modules/common/adapters:adapter_manager",
         "//modules/dreamview/backend/common:dreamview_gflags",
         "//modules/dreamview/backend/map:map_service",
+        "//modules/map/relative_map/proto:navigation_proto",
         "@gtest//:gtest",
     ],
 )

modules/dreamview/backend/sim_control/sim_control.cc

@@ -75,11 +75,14 @@ void SimControl::Init(bool set_start_point, double start_velocity,
   AdapterManager::AddPlanningCallback(&SimControl::OnPlanning, this);
   AdapterManager::AddRoutingResponseCallback(&SimControl::OnRoutingResponse,
                                              this);
+  AdapterManager::AddNavigationCallback(&SimControl::OnReceiveNavigationInfo,
+                                        this);
+
   // Start timer to publish localization and chassis messages.
   sim_control_timer_ = AdapterManager::CreateTimer(
       ros::Duration(kSimControlInterval), &SimControl::TimerCallback, this);
 
-  if (set_start_point) {
+  if (set_start_point && !FLAGS_use_navigation_mode) {
     apollo::common::PointENU start_point;
     if (!map_service_->GetStartPoint(&start_point)) {
       AWARN << "Failed to get a dummy start point from map!";
@@ -92,6 +95,17 @@ void SimControl::Init(bool set_start_point, double start_velocity,
   start_acceleration_ = start_acceleration;
 }
 
+void SimControl::OnReceiveNavigationInfo(
+    const relative_map::NavigationInfo& navigation_info) {
+  navigation_info_ = navigation_info;
+  if (navigation_info_.navigation_path_size() > 0) {
+    const auto& path = navigation_info_.navigation_path(0).path();
+    if (path.path_point_size() > 0) {
+      adc_position_ = path.path_point(0);
+    }
+  }
+}
+
 void SimControl::SetStartPoint(const double x, const double y) {
   next_point_.set_v(start_velocity_);
   next_point_.set_a(start_acceleration_);
@@ -291,11 +305,22 @@ void SimControl::PublishLocalization(double lambda) {
   pose->mutable_position()->set_y(cur_y);
   double cur_z = Interpolate(prev.z(), next.z(), lambda);
   pose->mutable_position()->set_z(cur_z);
-
   // Set orientation and heading
   double cur_theta = NormalizeAngle(
       prev.theta() + lambda * NormalizeAngle(next.theta() - prev.theta()));
 
+  if (FLAGS_use_navigation_mode) {
+    double flu_x = cur_x;
+    double flu_y = cur_y;
+    double enu_x = 0.0;
+    double enu_y = 0.0;
+    common::math::RotateAxis(-cur_theta, flu_x, flu_y, &enu_x, &enu_y);
+    enu_x += adc_position_.x();
+    enu_y += adc_position_.y();
+    pose->mutable_position()->set_x(enu_x);
+    pose->mutable_position()->set_y(enu_y);
+  }
+
   Eigen::Quaternion<double> cur_orientation =
       HeadingToQuaternion<double>(cur_theta);
   pose->mutable_orientation()->set_qw(cur_orientation.w());
@@ -333,6 +358,10 @@ void SimControl::PublishLocalization(double lambda) {
                  pose->mutable_linear_acceleration_vrf());
 
   AdapterManager::PublishLocalization(localization);
+
+  adc_position_.set_x(pose->position().x());
+  adc_position_.set_y(pose->position().y());
+  adc_position_.set_z(pose->position().z());
 }
 
 }  // namespace dreamview

modules/dreamview/backend/sim_control/sim_control.h

@@ -28,6 +28,7 @@
 #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
@@ -85,6 +86,8 @@ class SimControl : SimControlInterface {
  private:
   void OnPlanning(const apollo::planning::ADCTrajectory &trajectory);
   void OnRoutingResponse(const apollo::routing::RoutingResponse &routing);
+  void OnReceiveNavigationInfo(
+      const relative_map::NavigationInfo &navigation_info);
 
   // Reset the start point, which can be a dummy point on the map or received
   // from the routing module.
@@ -140,12 +143,16 @@ class SimControl : SimControlInterface {
   apollo::common::TrajectoryPoint prev_point_;
   apollo::common::TrajectoryPoint next_point_;
 
+  common::PathPoint adc_position_;
+
   // Initial velocity and acceleration of the main vehicle
   double start_velocity_ = 0.0;
   double start_acceleration_ = 0.0;
 
   static constexpr int kPlanningCountToStart = 5;
 
+  relative_map::NavigationInfo navigation_info_;
+
   FRIEND_TEST(SimControlTest, Test);
 };
 

modules/dreamview/backend/sim_control/sim_control_test.cc

@@ -80,6 +80,14 @@ class SimControlTest : public ::testing::Test {
           apollo::common::adapter::AdapterConfig::ROUTING_RESPONSE);
     }
 
+    {
+      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);
+    }
+
     AdapterManager::Init(config);
 
     sim_control_->Start();

modules/map/relative_map/navigation_lane.cc

@@ -160,12 +160,12 @@ void NavigationLane::ConvertNavigationLineToPath(common::Path *path) {
     point->CopyFrom(navigation_path.path_point(i));
 
     // shift to (0, 0)
-    double emu_x = point->x() + dx;
-    double emu_y = point->y() + dy;
+    double enu_x = point->x() + dx;
+    double enu_y = point->y() + dy;
 
     double flu_x = 0.0;
     double flu_y = 0.0;
-    common::math::RotateAxis(original_pose_.heading(), emu_x, emu_y, &flu_x,
+    common::math::RotateAxis(original_pose_.heading(), enu_x, enu_y, &flu_x,
                              &flu_y);
 
     point->set_x(flu_x);