diff --git a/modules/common/vehicle_state/BUILD b/modules/common/vehicle_state/BUILD index f5247fd903c287ea52397fec4a1b8992bdff22c3..f814a9f0e818a473ed96b937f9d10541e69f8759 100644 --- a/modules/common/vehicle_state/BUILD +++ b/modules/common/vehicle_state/BUILD @@ -14,6 +14,7 @@ cc_library( "//modules/canbus/proto:canbus_proto", "//modules/common:log", "//modules/common/math:quaternion", + "//modules/common/util", "//modules/localization/common:localization_common", "@eigen//:eigen", ], diff --git a/modules/common/vehicle_state/vehicle_state.cc b/modules/common/vehicle_state/vehicle_state.cc index 5bd207a66c6d39a3f51476c385cc6e2110dee193..a2af942f7cde13d4f33e35ffbfef451cdbec2e05 100644 --- a/modules/common/vehicle_state/vehicle_state.cc +++ b/modules/common/vehicle_state/vehicle_state.cc @@ -20,6 +20,7 @@ #include "modules/common/log.h" #include "modules/common/math/quaternion.h" +#include "modules/common/util/file.h" #include "modules/common/vehicle_state/vehicle_state.h" #include "modules/localization/common/localization_gflags.h" @@ -28,6 +29,23 @@ namespace common { VehicleState::VehicleState() {} +bool VehicleState::Update(const std::string &localization_file, + const std::string &chassis_file) { + localization::LocalizationEstimate localization; + if (!common::util::GetProtoFromFile(localization_file, &localization)) { + AERROR << "Failed to load file " << localization_file; + return false; + } + + canbus::Chassis chassis; + if (!common::util::GetProtoFromFile(chassis_file, &chassis)) { + AERROR << "Failed to load file " << chassis_file; + return false; + } + Update(localization, chassis); + return true; +} + void VehicleState::Update( const localization::LocalizationEstimate &localization, const canbus::Chassis &chassis) { diff --git a/modules/common/vehicle_state/vehicle_state.h b/modules/common/vehicle_state/vehicle_state.h index 27c6444b5d6b19348af57907d1cec7f286d6fe61..af5a1312dc0a6fa89474449086dc4e533c3968d1 100644 --- a/modules/common/vehicle_state/vehicle_state.h +++ b/modules/common/vehicle_state/vehicle_state.h @@ -49,6 +49,15 @@ class VehicleState { void Update(const localization::LocalizationEstimate& localization, const canbus::Chassis& chassis); + /** + * @brief Update localization from protobuf files. + * @param localization The localization protobuf file + * @param chassis The chassis protobuf file + * @return true if update success. + */ + bool Update(const std::string& localization_file, + const std::string& chassis_file); + double timestamp() const { return timestamp_; }; const apollo::localization::Pose& pose() const { return pose_; }; diff --git a/modules/planning/optimizer/BUILD b/modules/planning/optimizer/BUILD index c90b35ec4567357cf5c2cd8356fe16afaced1131..a34de6e80a934d00cfc699e86bbe1ad71af9eb84 100644 --- a/modules/planning/optimizer/BUILD +++ b/modules/planning/optimizer/BUILD @@ -71,6 +71,7 @@ cc_library( "//modules/common:log", "//modules/common/adapters:adapter_manager", "//modules/common/util", + "//modules/common/vehicle_state", "//modules/planning/common:data_center", "//modules/planning/proto:dp_poly_path_config_proto", "@gtest//:gtest", diff --git a/modules/planning/optimizer/dp_poly_path/dp_road_graph_test.cc b/modules/planning/optimizer/dp_poly_path/dp_road_graph_test.cc index 8fcc70c9e9a8d93fa941ffcd57903a2bdbe45e44..ae7c83682506605afc28e52811ff72c21e0f0bee 100644 --- a/modules/planning/optimizer/dp_poly_path/dp_road_graph_test.cc +++ b/modules/planning/optimizer/dp_poly_path/dp_road_graph_test.cc @@ -14,6 +14,8 @@ * limitations under the License. *****************************************************************************/ +#include +#include #include #include "gmock/gmock.h" @@ -39,11 +41,13 @@ using common::adapter::AdapterManager; class DpRoadGraphTest : public OptimizerTestBase { public: - void SetInitPointWithVelocity(const double velocity) { - init_point_.mutable_path_point()->set_x(586392.84003); - init_point_.mutable_path_point()->set_y(4140673.01232); - init_point_.set_v(velocity); - init_point_.set_a(0.0); + void SetInitPoint() { + init_point_.mutable_path_point()->set_x(pose_.position().x()); + init_point_.mutable_path_point()->set_y(pose_.position().y()); + const auto& velocity = pose_.linear_velocity(); + init_point_.set_v(std::hypot(velocity.x(), velocity.y())); + const auto& acc = pose_.linear_acceleration(); + init_point_.set_a(std::hypot(acc.x(), acc.y())); init_point_.set_relative_time(0.0); } @@ -66,6 +70,8 @@ class DpRoadGraphTest : public OptimizerTestBase { virtual void SetUp() { google::InitGoogleLogging("DpRoadGraphTest"); OptimizerTestBase::SetUp(); + SetInitPoint(); + SetSpeedDataWithConstVeolocity(10.0); reference_line_ = &(frame_->planning_data().reference_line()); } @@ -78,17 +84,14 @@ class DpRoadGraphTest : public OptimizerTestBase { }; TEST_F(DpRoadGraphTest, speed_road_graph) { - SetInitPointWithVelocity(10.0); - SetSpeedDataWithConstVeolocity(10.0); - DpRoadGraph road_graph(dp_poly_path_config_, init_point_, speed_data_); ASSERT_TRUE(reference_line_ != nullptr); bool result = road_graph.find_tunnel(*reference_line_, &decision_data_, &path_data_); EXPECT_TRUE(result); - EXPECT_EQ(808, path_data_.discretized_path().num_of_points()); - EXPECT_EQ(808, path_data_.frenet_frame_path().number_of_points()); - EXPECT_FLOAT_EQ(80.0, path_data_.frenet_frame_path().points().back().s()); + EXPECT_EQ(407, path_data_.discretized_path().num_of_points()); + EXPECT_EQ(407, path_data_.frenet_frame_path().number_of_points()); + EXPECT_FLOAT_EQ(40.0, path_data_.frenet_frame_path().points().back().s()); EXPECT_FLOAT_EQ(0.0, path_data_.frenet_frame_path().points().back().l()); // export_path_data(path_data_, "/tmp/path.txt"); } diff --git a/modules/planning/optimizer/optimizer_test_base.cc b/modules/planning/optimizer/optimizer_test_base.cc index 0769dd3ebe1a6428a466ed03ae91404ecd333f54..193e0b442b17ff765ac2c518aa66c558eda6842c 100644 --- a/modules/planning/optimizer/optimizer_test_base.cc +++ b/modules/planning/optimizer/optimizer_test_base.cc @@ -17,6 +17,7 @@ #include "modules/planning/optimizer/optimizer_test_base.h" #include "modules/common/log.h" +#include "modules/common/vehicle_state/vehicle_state.h" namespace apollo { namespace planning { @@ -26,6 +27,12 @@ using common::adapter::AdapterManager; DEFINE_string(test_routing_result_file, "modules/planning/testdata/garage_routing.pb.txt", "The routing file used in test"); +DEFINE_string(test_localization_file, + "modules/planning/testdata/garage_localization.pb.txt", + "The localization test file"); +DEFINE_string(test_chassis_file, + "modules/planning/testdata/garage_chassis.pb.txt", + "The chassis test file"); void OptimizerTestBase::SetDataConfigs() { FLAGS_planning_config_file = @@ -36,6 +43,9 @@ void OptimizerTestBase::SetDataConfigs() { "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"; + FLAGS_test_localization_file = + "modules/planning/testdata/garage_localization.pb.txt"; + FLAGS_test_chassis_file = "modules/planning/testdata/garage_chassis.pb.txt", FLAGS_v = 4; FLAGS_alsologtostderr = true; } @@ -61,6 +71,16 @@ void OptimizerTestBase::SetUp() { AERROR << "Failed to load file " << FLAGS_dp_poly_path_config_file; return; } + if (AdapterManager::GetLocalization()->Empty() || + AdapterManager::GetChassis()->Empty()) { + common::VehicleState::instance()->Update(FLAGS_test_localization_file, + FLAGS_test_chassis_file); + } else { + common::VehicleState::instance()->Update( + AdapterManager::GetLocalization()->GetLatestObserved(), + AdapterManager::GetChassis()->GetLatestObserved()); + } + pose_ = common::VehicleState::instance()->pose(); frame_ = data_center->current_frame(); ASSERT_TRUE(frame_ != nullptr); } diff --git a/modules/planning/optimizer/optimizer_test_base.h b/modules/planning/optimizer/optimizer_test_base.h index 9d11d73800424962e7be2673a1cafbc3545f66af..3fc0c7c64e2ac804bc418b46439279ba683efadd 100644 --- a/modules/planning/optimizer/optimizer_test_base.h +++ b/modules/planning/optimizer/optimizer_test_base.h @@ -35,6 +35,8 @@ namespace planning { using common::adapter::AdapterManager; DECLARE_string(test_routing_result_file); +DECLARE_string(test_localization_file); +DECLARE_string(test_chassis_file); class OptimizerTestBase : public ::testing::Test { public: @@ -68,6 +70,7 @@ class OptimizerTestBase : public ::testing::Test { protected: DpPolyPathConfig dp_poly_path_config_; + localization::Pose pose_; Frame* frame_ = nullptr; }; diff --git a/modules/planning/testdata/garage_chassis.pb.txt b/modules/planning/testdata/garage_chassis.pb.txt new file mode 100644 index 0000000000000000000000000000000000000000..24d34205b51e380d9273e269d7584bf1e3a10523 --- /dev/null +++ b/modules/planning/testdata/garage_chassis.pb.txt @@ -0,0 +1,22 @@ +engine_started: true +engine_rpm: 0.0 +speed_mps: 0.0 +odometer_m: 0.0 +fuel_range_m: 0 +throttle_percentage: 15.2391853333 +brake_percentage: 13.6079959869 +steering_percentage: 5.65957450867 +steering_torque_nm: -0.3125 +parking_brake: false +driving_mode: COMPLETE_MANUAL +error_code: NO_ERROR +gear_location: GEAR_NEUTRAL +header { + timestamp_sec: 1498682801.33 + module_name: "chassis" + sequence_num: 169202 +} +signal { + turn_signal: TURN_NONE + horn: false +} diff --git a/modules/planning/testdata/garage_localization.pb.txt b/modules/planning/testdata/garage_localization.pb.txt new file mode 100644 index 0000000000000000000000000000000000000000..2ea15c0b1548f14ea746b9c89799b084c3318e7d --- /dev/null +++ b/modules/planning/testdata/garage_localization.pb.txt @@ -0,0 +1,44 @@ +header { + timestamp_sec: 1498682801.33 + module_name: "localization" + sequence_num: 168770 +} +pose { + position { + x: 586387.203263 + y: 4140676.21271 + z: -29.1851183716 + } + orientation { + qx: -0.00551346339344 + qy: 0.0363995342758 + qz: -0.620280830689 + qw: -0.783515390216 + } + linear_velocity { + x: 0.00149997781434 + y: 0.000880812599767 + z: -0.00122802127571 + } + linear_acceleration { + x: -0.00582869976263 + y: 0.0100514010432 + z: 0.0235518746941 + } + angular_velocity { + x: 0.000998771709994 + y: 0.000154125122251 + z: 1.03917169376e-05 + } + heading: 2.90890211889 + linear_acceleration_vrf { + x: 0.00994229889291 + y: 0.00712407897059 + z: 0.0232399095739 + } + angular_velocity_vrf { + x: 0.000377985098552 + y: -0.000936068612645 + z: -4.80654498315e-05 + } +}