added path_sampler_test

modules/planning/common/data_center.cc

@@ -114,9 +114,8 @@ bool DataCenter::CreateReferenceLineFromMap() {
   return true;
 }
 
-bool DataCenter::init_frame(const uint32_t sequence_num) {
+bool DataCenter::init_current_frame(const uint32_t sequence_num) {
   _frame.reset(new Frame(sequence_num));
-  _frame->set_planning_data(new PlanningData());
 
   if (!CreateReferenceLineFromMap()) {
     AERROR << "failed to create reference line";

modules/planning/common/data_center.h

@@ -41,7 +41,7 @@ class DataCenter {
   Frame *frame(const uint32_t sequence_num) const;
 
  public:
-  bool init_frame(const uint32_t sequence_num);
+  bool init_current_frame(const uint32_t sequence_num);
   Frame *current_frame() const;
   void save_frame();
   const ::apollo::hdmap::HDMap &map() { return map_; }

modules/planning/common/frame.cc

@@ -33,21 +33,11 @@ void Frame::set_sequence_num(const uint32_t sequence_num) {
   _sequence_num = sequence_num;
 }
 
-void Frame::set_planning_data(PlanningData *const planning_data) {
-  _planning_data.reset(planning_data);
-}
-
 uint32_t Frame::sequence_num() const { return _sequence_num; }
 
-const PlanningData &Frame::planning_data() const {
-  CHECK_NOTNULL(_planning_data.get());
-  return *(_planning_data.get());
-}
+const PlanningData &Frame::planning_data() const { return _planning_data; }
 
-PlanningData *Frame::mutable_planning_data() {
-  CHECK_NOTNULL(_planning_data.get());
-  return _planning_data.get();
-}
+PlanningData *Frame::mutable_planning_data() { return &_planning_data; }
 
 void Frame::set_computed_trajectory(const PublishableTrajectory &trajectory) {
   _computed_trajectory = trajectory;

modules/planning/common/frame.h

@@ -35,7 +35,6 @@ class Frame {
   explicit Frame(uint32_t sequence_num);
 
   void set_sequence_num(const uint32_t sequence_num);
-  void set_planning_data(PlanningData *const planning_data);
 
   uint32_t sequence_num() const;
   const PlanningData &planning_data() const;
@@ -49,7 +48,7 @@ class Frame {
  private:
   uint32_t _sequence_num;
   PublishableTrajectory _computed_trajectory;
-  std::unique_ptr<PlanningData> _planning_data;
+  PlanningData _planning_data;
 };
 
 }  // namespace planning

modules/planning/optimizer/dp_poly_path/BUILD

@@ -54,4 +54,25 @@ cc_library(
     ],
 )
 
+cc_test(
+    name = "path_sampler_test",
+    size = "small",
+    srcs = [
+        "path_sampler_test.cc",
+    ],
+    data = [
+        "//modules/map:map_data",
+        "//modules/planning:planning_testdata",
+    ],
+    deps = [
+        ":dp_poly_path",
+        "//modules/common:log",
+        "//modules/common/adapters:adapter_manager",
+        "//modules/common/util",
+        "//modules/planning/common:data_center",
+        "//modules/planning/proto:dp_poly_path_config_proto",
+        "@gtest//:main",
+    ],
+)
+
 cpplint()

modules/planning/optimizer/dp_poly_path/dp_road_graph.cc

@@ -183,8 +183,8 @@ bool DpRoadGraph::init(const ReferenceLine &reference_line) {
 Status DpRoadGraph::generate_graph(const ReferenceLine &reference_line) {
   std::vector<std::vector<common::SLPoint>> points;
   PathSampler path_sampler(_config);
-  if (!path_sampler.sample(reference_line, _init_point, _init_sl_point, &points)
-           .ok()) {
+  if (!path_sampler.sample(reference_line, _init_point, _init_sl_point,
+                           &points)) {
     const std::string msg = "Fail to sampling point with path sampler!";
     AERROR << msg;
     return Status(ErrorCode::PLANNING_ERROR, msg);

modules/planning/optimizer/dp_poly_path/path_sampler.cc

@@ -36,7 +36,7 @@ using SLPoint = apollo::common::SLPoint;
 
 PathSampler::PathSampler(const DpPolyPathConfig& config) : _config(config) {}
 
-Status PathSampler::sample(
+bool PathSampler::sample(
     const ReferenceLine& reference_line,
     const ::apollo::common::TrajectoryPoint& init_point,
     const ::apollo::common::SLPoint& init_sl_point,
@@ -67,8 +67,7 @@ Status PathSampler::sample(
     }
     points->push_back(level_points);
   }
-
-  return Status::OK();
+  return true;
 }
 }  // namespace planning
 }  // namespace apollo

modules/planning/optimizer/dp_poly_path/path_sampler.h

@@ -34,11 +34,10 @@ class PathSampler {
  public:
   explicit PathSampler(const DpPolyPathConfig &config);
   ~PathSampler() = default;
-  apollo::common::Status sample(
-      const ReferenceLine &reference_line,
-      const ::apollo::common::TrajectoryPoint &init_point,
-      const common::SLPoint &init_sl_point,
-      std::vector<std::vector<common::SLPoint>> *const points);
+  bool sample(const ReferenceLine &reference_line,
+              const common::TrajectoryPoint &init_point,
+              const common::SLPoint &init_sl_point,
+              std::vector<std::vector<common::SLPoint>> *const points);
 
  private:
   DpPolyPathConfig _config;

modules/planning/optimizer/dp_poly_path/path_sampler_test.cc

+/******************************************************************************
+ * 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 "gmock/gmock.h"
+#include "gtest/gtest.h"
+#include "ros/include/ros/ros.h"
+
+#include "modules/planning/proto/dp_poly_path_config.pb.h"
+
+#include "modules/common/adapters/adapter_gflags.h"
+#include "modules/common/adapters/adapter_manager.h"
+#include "modules/common/log.h"
+#include "modules/common/util/file.h"
+#include "modules/planning/common/data_center.h"
+#include "modules/planning/common/planning_gflags.h"
+#include "modules/planning/optimizer/dp_poly_path/path_sampler.h"
+#include "modules/planning/optimizer/dp_poly_path/path_sampler.h"
+
+namespace apollo {
+namespace planning {
+
+using common::adapter::AdapterManager;
+
+class PathSamplerTest : public ::testing::Test {
+ public:
+  virtual void SetUp() {
+    FLAGS_planning_config_file =
+        "modules/planning/testdata/conf/planning_config.pb.txt";
+    FLAGS_adapter_config_path = "modules/planning/testdata/conf/adapter.conf";
+    FLAGS_map_filename = "modules/planning/testdata/base_map.txt";
+    FLAGS_reference_line_smoother_config_file =
+        "modules/planning/testdata/conf/reference_line_smoother_config.pb.txt";
+    FLAGS_dp_poly_path_config_file =
+        "modules/planning/testdata/conf/dp_poly_path_config.pb.txt";
+    AdapterManager::Init(FLAGS_adapter_config_path);
+    if (!AdapterManager::GetRoutingResult()) {
+      AERROR << "routing is not registered in adapter manager, check adapter "
+                "config file "
+             << FLAGS_adapter_config_path;
+      return;
+    }
+    AdapterManager::FeedRoutingResultFile(routing_file_);
+    AdapterManager::Observe();
+    auto* data_center = DataCenter::instance();
+    if (!data_center->init_current_frame(0)) {
+      AERROR << "Failed to init frame";
+      return;
+    }
+    if (!common::util::GetProtoFromFile(FLAGS_dp_poly_path_config_file,
+                                        &config_)) {
+      AERROR << "Failed to load file " << FLAGS_dp_poly_path_config_file;
+      return;
+    }
+    frame_ = data_center->current_frame();
+  }
+
+  void export_sl_points(std::vector<std::vector<common::SLPoint>>& points,
+                        const std::string& filename) {
+    std::ofstream ofs(filename);
+    ofs << "level, s, l" << std::endl;
+    int level = 0;
+    for (const auto& level_points : points) {
+      for (const auto& point : level_points) {
+        ofs << level << ", " << point.s() << ", " << point.l() << std::endl;
+      }
+      ++level;
+    }
+    ofs.close();
+  }
+
+ protected:
+  const std::string routing_file_ =
+      "modules/planning/testdata/garage_routing.pb.txt";
+  DpPolyPathConfig config_;
+  Frame* frame_ = nullptr;
+};
+
+TEST_F(PathSamplerTest, sample_one_point) {
+  ASSERT_TRUE(frame_ != nullptr);
+  config_.set_sample_points_num_each_level(1);
+  PathSampler sampler(config_);
+  std::vector<std::vector<common::SLPoint>> sampled_points;
+  common::TrajectoryPoint init_point;
+  init_point.mutable_path_point()->set_x(586392.84003);
+  init_point.mutable_path_point()->set_y(4140673.01232);
+  common::SLPoint init_sl_point;
+  init_sl_point.set_s(0.0);
+  init_sl_point.set_l(0.0);
+
+  const auto& reference_line = frame_->planning_data().reference_line();
+  const double reference_line_length =
+      reference_line.reference_map_line().length();
+  EXPECT_FLOAT_EQ(70.1723, reference_line_length);
+  bool sample_result = sampler.sample(reference_line, init_point, init_sl_point,
+                                      &sampled_points);
+  EXPECT_TRUE(sample_result);
+  EXPECT_EQ(8, sampled_points.size());
+  EXPECT_EQ(1, sampled_points[0].size());
+  EXPECT_EQ(1, sampled_points[7].size());
+  EXPECT_FLOAT_EQ(40, sampled_points[7][0].s());
+  EXPECT_FLOAT_EQ(0, sampled_points[7][0].l());
+  // export_sl_points(sampled_points, "/tmp/points.txt");
+}
+
+TEST_F(PathSamplerTest, sample_three_points) {
+  ASSERT_TRUE(frame_ != nullptr);
+  config_.set_sample_points_num_each_level(3);
+  PathSampler sampler(config_);
+  std::vector<std::vector<common::SLPoint>> sampled_points;
+  common::TrajectoryPoint init_point;
+  init_point.mutable_path_point()->set_x(586392.84003);
+  init_point.mutable_path_point()->set_y(4140673.01232);
+  common::SLPoint init_sl_point;
+  init_sl_point.set_s(0.0);
+  init_sl_point.set_l(0.0);
+
+  const auto& reference_line = frame_->planning_data().reference_line();
+  const double reference_line_length =
+      reference_line.reference_map_line().length();
+  EXPECT_FLOAT_EQ(70.1723, reference_line_length);
+  bool sample_result = sampler.sample(reference_line, init_point, init_sl_point,
+                                      &sampled_points);
+  EXPECT_TRUE(sample_result);
+  EXPECT_EQ(8, sampled_points.size());
+  EXPECT_EQ(3, sampled_points[0].size());
+  ASSERT_EQ(3, sampled_points[7].size());
+  EXPECT_FLOAT_EQ(40, sampled_points[7][0].s());
+  EXPECT_FLOAT_EQ(-0.5, sampled_points[7][0].l());
+  EXPECT_FLOAT_EQ(40, sampled_points[7][1].s());
+  EXPECT_FLOAT_EQ(0, sampled_points[7][1].l());
+  EXPECT_FLOAT_EQ(40, sampled_points[7][2].s());
+  EXPECT_FLOAT_EQ(0.5, sampled_points[7][2].l());
+  // export_sl_points(sampled_points, "/tmp/points.txt");
+}
+
+}  // namespace planning
+}  // namespace apollo

modules/planning/planning.cc

@@ -139,7 +139,7 @@ void Planning::RunOnce() {
       apollo::common::time::ToSecond(apollo::common::time::Clock::Now()) +
       planning_cycle_time;
 
-  if (!DataCenter::instance()->init_frame(
+  if (!DataCenter::instance()->init_current_frame(
           AdapterManager::GetPlanning()->GetSeqNum() + 1)) {
     AERROR << "DataCenter init frame failed";
     return;

modules/planning/testdata/garage_routing.pb.txt

+header {
+  module_name: "routing"
+  timestamp_sec: 1234.5
+  sequence_num: 1
+}
+routing_request {
+    end {
+        id: "1_-1"
+        s: 153.87421245705966
+        pose {
+            x: 200
+            y: 2
+        }
+    }
+}
+route {
+      id: "1_-1"
+      start_s: 0.0
+      end_s: 153.87421245705966
+}
+measurement {
+     distance: 153.87421245705966
+}