apollo自动驾驶进阶学习之：轨迹信息内容填充

    cc_test(
    name = "reference_line_provider_test",
    size = "small",
    srcs = ["reference_line_provider_test.cc"],
    data = [
         "//modules/map/data:map_sunnyvale_loop",
         ":localization_testdata",
    ],
    deps = [
        "//cyber/common:log",
        ":reference_line_provider",
        "//modules/common/util",
        "//modules/planning/common:planning_common",
        "@com_google_googletest//:gtest_main",
        "//modules/common/vehicle_state:vehicle_state_provider",
        #"@local_config_python//:python_headers",
        #"@local_config_python//:python_lib",
        #"//modules/planning/proto:planning_cc_proto",
        #":matplotlibcpp"
    ],
    )

    /****************************************************************************** * 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 "modules/planning/reference_line/reference_line_provider.h"
    #include "gflags/gflags.h"
    #include "gtest/gtest.h"
    #include "cyber/common/file.h"
    #include "modules/common/util/string_util.h"
    #include "modules/common/vehicle_state/vehicle_state_provider.h"
    #include "modules/canbus/proto/chassis.pb.h"
    #include "modules/localization/common/localization_gflags.h"
    #include "modules/localization/proto/localization.pb.h"
    #include "modules/planning/common/planning_gflags.h"
    #include <iostream>
    #include "cyber/time/clock.h"
    //#include "matplotlibcpp.h"
    #include "cyber/common/log.h"
    #include "modules/planning/proto/planning.pb.h"
    using  namespace std;
    
    
    namespace apollo {
    namespace planning {
    
    using apollo::canbus::Chassis;
    using apollo::localization::LocalizationEstimate;
    using apollo::common::VehicleStateProvider;
    using apollo::hdmap::HDMapUtil;
    using apollo::cyber::Clock;
    
    DEFINE_string(test_map_file,
              "modules/map/data/sunnyvale_loop/base_map_test.bin",
              "The test map file");
    DEFINE_string(
    test_routing_file,
    "modules/planning/reference_line/testdata/sample_sunnyvale_loop_routing.pb.txt",
    "The test map file");
    
    class ReferenceLineProviderTest : public ::testing::Test {
     public:
      virtual void SetUp() {
    FLAGS_enable_reference_line_provider_thread = true;
    AINFO << "map file: " << FLAGS_test_map_file;
    if (hdmap_.LoadMapFromFile(FLAGS_test_map_file) != 0) {
      AERROR << "Failed to load map: " << FLAGS_test_map_file;
      ACHECK(false);
    }
    
    if (!cyber::common::GetProtoFromFile(FLAGS_test_routing_file, &routing_)) {
      AERROR << "Failed to load routing: " << FLAGS_test_routing_file;
      ACHECK(false);
    }
    
    
    cout << "--------------------------------------------------"<<endl;
    cout << "map file loaded "<<endl;
    cout << "--------------------------------------------------"<<endl;
    
    std::string localization_file =
        "modules/planning/reference_line/testdata/3_localization_result_1.pb.txt";
    ACHECK(cyber::common::GetProtoFromFile(localization_file, &localization_));
    chassis_.set_speed_mps(3.0);
    chassis_.set_gear_location(canbus::Chassis::GEAR_DRIVE);
    FLAGS_enable_map_reference_unify = true;   
      } 
    
      
      
     protected:
      LocalizationEstimate localization_;
      Chassis chassis_;
      hdmap::HDMap hdmap_;
      unique_ptr<ReferenceLineProvider> reference_line_provider_;
      routing::RoutingResponse routing_;
    };
    
    
    TEST_F(ReferenceLineProviderTest, test) {
      cout << "--------------------------------------------------"<<endl;
      cout << "this is a test"<<endl;
      cout << "--------------------------------------------------"<<endl;
      auto vehicle_state_provider = VehicleStateProvider();
      vehicle_state_provider.Update(localization_, chassis_);
      cout<<vehicle_state_provider.x()<<endl;
      cout<<vehicle_state_provider.y()<<endl;
      cout<<vehicle_state_provider.heading()<<endl; 
      
      reference_line_provider_ = std::make_unique<ReferenceLineProvider>(&vehicle_state_provider, &hdmap_);
      reference_line_provider_->UpdateRoutingResponse(routing_);
      reference_line_provider_->Start();
    
      const double start_timestamp = Clock::NowInSeconds();
      
      
      //等待异步线程准备就绪
      size_t ref_size=0;
      std::list<ReferenceLine> reference_lines;
      std::list<hdmap::RouteSegments> segments;
      while (ref_size==0){
    reference_line_provider_->UpdateVehicleState(vehicle_state_provider.vehicle_state());
    
    reference_line_provider_->UpdatedReferenceLine();
    if (!reference_line_provider_->GetReferenceLines(&reference_lines,
                                                    &segments)) {
      const std::string msg = "Failed to create reference line";
      AERROR << msg;
    }
    
    cout << "--------------------------------------------------"<<endl;
    cout<<"cout<<reference_lines.size()<<endl: "<<reference_lines.size()<<endl;
    ref_size=reference_lines.size();
    cout << "--------------------------------------------------"<<endl;
      }
      cout<<"计算消耗时间use time: "<<Clock::NowInSeconds() - start_timestamp<<endl;
      reference_line_provider_->Stop();
      
      ADCTrajectory adc_trajectory_pb;
      for (auto& ref_line : reference_lines) {
      ref_line.DebugString_SS();
      cout<<"参考线长度："<<ref_line.Length()<<endl;
      const auto& reference_points=ref_line.reference_points();
      for(const auto& reference_point:reference_points)
      {
        auto *point= adc_trajectory_pb.add_trajectory_point();
        point->mutable_path_point()->set_x(reference_point.x());
        point->mutable_path_point()->set_y(reference_point.y());
        //std::cout.precision(5);
        //cout<<std::fixed<<reference_point.x()<<"  "<<reference_point.x()<<endl;
      }
      }
      cout<<"轨迹点数量: "<<adc_trajectory_pb.trajectory_point_size()<<endl;
      
      apollo::cyber::Init("referenceline_trajectory_test");
      auto node=apollo::cyber::CreateNode("trajectory_test");
      std::shared_ptr<apollo::cyber::Writer<ADCTrajectory>> planning_writer;
      planning_writer=node->CreateWriter<ADCTrajectory>("/apollo/planning");
      apollo::cyber::Rate rate(1.0);
      
      while(apollo::cyber::OK()){
    AINFO<<"循环发送报文中";
    planning_writer->Write(adc_trajectory_pb);
    rate.Sleep();
      }
     
    
      
      cout<<"TEST END"<<endl;
    
    }
    }  // namespace planning
    }  // namespace apollo