RelativeMap: configured downsampling parameters via a proto config file.

modules/map/relative_map/common/relative_map_gflags.cc

@@ -17,11 +17,16 @@
 #include "modules/map/relative_map/common/relative_map_gflags.h"
 
 DEFINE_string(relative_map_adapter_config_filename,
-              "modules/map/relative_map/conf/adapter.conf",
+              "/apollo/modules/map/relative_map/conf/adapter.conf",
               "gflags conf file for relative map");
-DEFINE_string(relative_map_config_filename,
-              "modules/map/relative_map/conf/relative_map_config.pb.txt",
-              "Relative map configuration file");
+DEFINE_string(
+    relative_map_config_filename,
+    "/apollo/modules/map/relative_map/conf/relative_map_config.pb.txt",
+    "Relative map configuration file");
+
+DEFINE_string(navigator_config_filename,
+              "/apollo/modules/map/relative_map/conf/navigator_config.pb.txt",
+              "navigator config file name.");
 
 DEFINE_int32(relative_map_loop_rate, 10, "Loop rate for relative_map node");
 
@@ -30,7 +35,3 @@ DEFINE_bool(enable_cyclic_rerouting, false,
 
 DEFINE_bool(relative_map_generate_left_boundray, true,
             "Generate left boundary for detected lanes.");
-
-DEFINE_bool(navigator_down_sample, true,
-            "When a navigation line is sent, the original data is downsampled "
-            "to reduce unnecessary memory consumption.");

modules/map/relative_map/common/relative_map_gflags.h

@@ -20,7 +20,7 @@
 
 DECLARE_string(relative_map_adapter_config_filename);
 DECLARE_string(relative_map_config_filename);
+DECLARE_string(navigator_config_filename);
 DECLARE_int32(relative_map_loop_rate);
 DECLARE_bool(enable_cyclic_rerouting);
 DECLARE_bool(relative_map_generate_left_boundray);
-DECLARE_bool(navigator_down_sample);

modules/map/relative_map/conf/navigator_config.pb.txt

+enable_navigator_downsample: true
+sample_param {
+  straight_sample_interval: 3.0
+  small_kappa_sample_interval: 1.0
+  middle_kappa_sample_interval: 0.4
+  large_kappa_sample_interval: 0.1
+  small_kappa: 0.002
+  middle_kappa: 0.008
+  large_kappa: 0.02
+}
\ No newline at end of file

modules/map/relative_map/proto/BUILD

@@ -37,4 +37,18 @@ proto_library(
     ],
 )
 
+cc_proto_library(
+    name = "navigator_config_proto",
+    deps = [
+        ":navigator_config_proto_lib",
+    ],
+)
+
+proto_library(
+    name = "navigator_config_proto_lib",
+    srcs = [
+        "navigator_config.proto",
+    ],
+)
+
 cpplint()

modules/map/relative_map/proto/navigator_config.proto

+syntax = "proto2";
+
+package apollo.relative_map;
+
+message SampleParam {
+  // Sampling interval for a straight line.
+  optional double straight_sample_interval = 1 [default = 3.0];
+  // Sampling interval for a curve segment with a small curvature.
+  optional double small_kappa_sample_interval = 2 [default = 1.0];
+  // Sampling interval for a curve segment with a middle curvature.
+  optional double middle_kappa_sample_interval = 3 [default = 0.4];
+  // Sampling interval for a curve segment with a large curvature.
+  optional double large_kappa_sample_interval = 4 [default = 0.1];
+  // Small curvature threshold.
+  optional double small_kappa = 5 [default = 0.002];
+  // Middle curvature threshold.
+  optional double middle_kappa = 6 [default = 0.008];
+  // Large curvature threshold.
+  optional double large_kappa = 7 [default = 0.02];
+}
+
+message NavigatorConfig {
+  // When a navigation line is sent, the original data is downsampled to reduce
+  // unnecessary memory consumption.
+  optional bool enable_navigator_downsample = 1 [default = true];
+  // Smapling paramters.
+  optional SampleParam sample_param = 2;
+}

modules/map/relative_map/tools/BUILD

@@ -23,6 +23,7 @@ cc_binary(
         "//modules/common/adapters:adapter_gflags",
         "//modules/common/util",
         "//modules/map/relative_map:relative_map_lib",
+        "//modules/map/relative_map/proto:navigator_config_proto",
         "//third_party/json",
     ],
 )

modules/map/relative_map/tools/navigator.cc

@@ -29,16 +29,20 @@
 #include "modules/common/util/message_util.h"
 #include "modules/map/relative_map/common/relative_map_gflags.h"
 #include "modules/map/relative_map/proto/navigation.pb.h"
+#include "modules/map/relative_map/proto/navigator_config.pb.h"
 
 using apollo::cyber::Rate;
 using apollo::relative_map::NavigationInfo;
 using apollo::relative_map::NavigationPath;
+using apollo::relative_map::NavigatorConfig;
 using nlohmann::json;
 
 bool ParseNavigationLineFileNames(
     int argc, char** argv, std::vector<std::string>* navigation_line_filenames);
 bool GetNavigationPathFromFile(const std::string& filename,
+                               const NavigatorConfig& navigator_config,
                                NavigationPath* navigation_path);
+void CheckConfig(const apollo::relative_map::NavigatorConfig& navigator_config);
 
 int main(int argc, char** argv) {
   google::ParseCommandLineFlags(&argc, &argv, true);
@@ -46,6 +50,16 @@ int main(int argc, char** argv) {
   apollo::cyber::Init(argv[0]);
   FLAGS_alsologtostderr = true;
 
+  NavigatorConfig navigator_config;
+  AINFO << "The navigator configuration filename is: "
+        << FLAGS_navigator_config_filename;
+  if (!apollo::common::util::GetProtoFromFile(FLAGS_navigator_config_filename,
+                                              &navigator_config)) {
+    AERROR << "Failed to parse " << FLAGS_navigator_config_filename;
+    return -1;
+  }
+  CheckConfig(navigator_config);
+
   std::vector<std::string> navigation_line_filenames;
   if (!ParseNavigationLineFileNames(argc, argv, &navigation_line_filenames)) {
     AERROR << "Failed to get navigation file names.";
@@ -59,13 +73,14 @@ int main(int argc, char** argv) {
   }
 
   ADEBUG << "The flag \"navigator_down_sample\" is: "
-         << FLAGS_navigator_down_sample;
+         << navigator_config.enable_navigator_downsample();
 
   NavigationInfo navigation_info;
   int i = 0;
   for (const std::string& filename : navigation_line_filenames) {
     auto* navigation_path = navigation_info.add_navigation_path();
-    if (!GetNavigationPathFromFile(filename, navigation_path)) {
+    if (!GetNavigationPathFromFile(filename, navigator_config,
+                                   navigation_path)) {
       AWARN << "Failed to load file: " << filename;
       continue;
     }
@@ -126,6 +141,7 @@ bool ParseNavigationLineFileNames(
 }
 
 bool GetNavigationPathFromFile(const std::string& filename,
+                               const NavigatorConfig& navigator_config,
                                NavigationPath* navigation_path) {
   CHECK_NOTNULL(navigation_path);
 
@@ -141,13 +157,7 @@ bool GetNavigationPathFromFile(const std::string& filename,
   double current_kappa = 0.0;
   int original_points_num = 0;
   int down_sampled_points_num = 0;
-  constexpr double kStraightSampleInterval = 3.0;
-  constexpr double kSmallKappaSampleInterval = 1.0;
-  constexpr double kMiddleKappaSampleInterval = 0.4;
-  constexpr double kLargeKappaSampleInterval = 0.1;
-  constexpr double kSmallKappa = 0.002;
-  constexpr double kMiddleKappa = 0.008;
-  constexpr double kLargeKappa = 0.02;
+  const auto& sample_param = navigator_config.sample_param();
   while (std::getline(ifs, line_str)) {
     try {
       auto json_obj = json::parse(line_str);
@@ -155,16 +165,18 @@ bool GetNavigationPathFromFile(const std::string& filename,
       current_kappa = json_obj["kappa"];
       diff_s = std::fabs(current_sampled_s - last_sampled_s);
       bool not_down_sampling =
-          FLAGS_navigator_down_sample
-              ? diff_s >= kStraightSampleInterval ||
-                    (diff_s >= kSmallKappaSampleInterval &&
-                     std::fabs(current_kappa) > kSmallKappa) ||
-                    (diff_s >= kMiddleKappaSampleInterval &&
-                     std::fabs(current_kappa) > kMiddleKappa) ||
-                    (diff_s >= kLargeKappaSampleInterval &&
-                     std::fabs(current_kappa) > kLargeKappa)
+          navigator_config.enable_navigator_downsample()
+              ? diff_s >= sample_param.straight_sample_interval() ||
+                    (diff_s >= sample_param.small_kappa_sample_interval() &&
+                     std::fabs(current_kappa) > sample_param.small_kappa()) ||
+                    (diff_s >= sample_param.middle_kappa_sample_interval() &&
+                     std::fabs(current_kappa) > sample_param.middle_kappa()) ||
+                    (diff_s >= sample_param.large_kappa_sample_interval() &&
+                     std::fabs(current_kappa) > sample_param.large_kappa())
               : true;
-      if (not_down_sampling) {
+      // Add a condition: !navigation_path->has_path() to keep the first point
+      // when down_sampling
+      if (not_down_sampling || !navigation_path->has_path()) {
         last_sampled_s = current_sampled_s;
         auto* point = navigation_path->mutable_path()->add_path_point();
         point->set_x(json_obj["x"]);
@@ -188,3 +200,16 @@ bool GetNavigationPathFromFile(const std::string& filename,
         << down_sampled_points_num << " in the file: " << filename;
   return true;
 }
+
+void CheckConfig(
+    const apollo::relative_map::NavigatorConfig& navigator_config) {
+  CHECK(navigator_config.has_sample_param());
+  const auto& sample_param = navigator_config.sample_param();
+  CHECK(sample_param.has_straight_sample_interval());
+  CHECK(sample_param.has_small_kappa_sample_interval());
+  CHECK(sample_param.has_middle_kappa_sample_interval());
+  CHECK(sample_param.has_large_kappa_sample_interval());
+  CHECK(sample_param.has_small_kappa());
+  CHECK(sample_param.has_middle_kappa());
+  CHECK(sample_param.has_large_kappa());
+}