提交 76b215a9 编写于 作者: D Dong Li 提交者: Aaron Xiao

remove duplicated tests that can be covered by integration tests

上级 6ac48c97
......@@ -41,7 +41,7 @@ class GarageTest : public PlanningTestBase {
public:
virtual void SetUp() {
FLAGS_map_file_path = "modules/planning/testdata/base_map.txt";
FLAGS_test_data_dir = "modules/planning/testdata/garage_test/";
FLAGS_test_data_dir = "modules/planning/testdata/garage_test";
}
};
......
......@@ -45,20 +45,4 @@ cc_library(
],
)
cc_test(
name = "dp_road_graph_test",
size = "small",
srcs = [
"dp_road_graph_test.cc",
],
data = [
"//modules/map:map_data",
"//modules/planning:planning_testdata",
],
deps = [
":dp_poly_path",
"//modules/planning/integration_tests:planning_test_base",
],
)
cpplint()
/******************************************************************************
* 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 <algorithm>
#include <cmath>
#include "gmock/gmock.h"
#include "gtest/gtest.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/planning_gflags.h"
#include "modules/planning/integration_tests/planning_test_base.h"
#include "modules/planning/optimizer/dp_poly_path/dp_road_graph.h"
namespace apollo {
namespace planning {
using common::adapter::AdapterManager;
class DpRoadGraphTest : public PlanningTestBase {
public:
void SetInitPoint() {
const auto* frame = planning_.GetFrame();
const auto& pose = frame->VehicleInitPose();
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);
}
void SetSpeedDataWithConstVeolocity(const double velocity) {
// speed point params: s, time, v, a, da
double t = 0.0;
const double delta_s = 1.0;
if (velocity > 0.0) {
for (double s = 0.0; s < 100.0; s += delta_s) {
speed_data_.AppendSpeedPoint(s, t, velocity, 0.0, 0.0);
t += delta_s / velocity;
}
} else { // when velocity = 0, stand still
for (double t = 0.0; t < 10.0; t += 0.1) {
speed_data_.AppendSpeedPoint(0.0, t, 0.0, 0.0, 0.0);
}
}
}
virtual void SetUp() {
FLAGS_test_data_dir = "modules/planning/testdata/dp_road_graph_test";
}
protected:
const ReferenceLine* reference_line_ = nullptr;
common::TrajectoryPoint init_point_;
SpeedData speed_data_; // input
PathData path_data_; // output
};
TEST_F(DpRoadGraphTest, speed_road_graph) {
PlanningTestBase::SetUp();
RUN_GOLDEN_TEST;
// specialized tests
SetInitPoint();
SetSpeedDataWithConstVeolocity(10.0);
const auto* frame = planning_.GetFrame();
ASSERT_TRUE(frame != nullptr);
reference_line_ = &(frame->reference_line());
DpPolyPathConfig dp_poly_path_config; // default values
DPRoadGraph road_graph(dp_poly_path_config, *reference_line_, speed_data_);
ASSERT_TRUE(reference_line_ != nullptr);
bool result = road_graph.FindPathTunnel(init_point_, &path_data_);
EXPECT_TRUE(result);
EXPECT_EQ(648, path_data_.discretized_path().NumOfPoints());
EXPECT_EQ(648, path_data_.frenet_frame_path().NumOfPoints());
EXPECT_FLOAT_EQ(70.450378,
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");
}
} // namespace planning
} // namespace apollo
TMAIN;
\ No newline at end of file
......@@ -63,20 +63,4 @@ cc_library(
],
)
cc_test(
name = "dp_st_graph_test",
size = "small",
srcs = [
"dp_st_graph_test.cc",
],
data = [
"//modules/map:map_data",
"//modules/planning:planning_testdata",
],
deps = [
":dp_st_graph",
"//modules/planning/integration_tests:planning_test_base",
],
)
cpplint()
/******************************************************************************
* 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 <algorithm>
#include <cmath>
#include "gmock/gmock.h"
#include "gtest/gtest.h"
#include "modules/planning/proto/dp_st_speed_config.pb.h"
#include "modules/common/adapters/adapter_gflags.h"
#include "modules/common/adapters/adapter_manager.h"
#include "modules/common/configs/vehicle_config_helper.h"
#include "modules/common/log.h"
#include "modules/common/util/file.h"
#include "modules/planning/common/path_decision.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/integration_tests/planning_test_base.h"
#include "modules/planning/optimizer/dp_st_speed/dp_st_graph.h"
namespace apollo {
namespace planning {
using common::adapter::AdapterManager;
class DpStSpeedTest : public PlanningTestBase {
public:
void SetPathDataWithStraightLine() {
double l = 0.0;
const double ds = 1.0;
std::vector<common::FrenetFramePoint> points;
for (double s = 0.0; s < 50.0; s += ds) {
common::FrenetFramePoint ffp;
ffp.set_s(s);
ffp.set_l(l);
points.push_back(ffp);
}
FrenetFramePath path(points);
path_data_.SetReferenceLine(reference_line_);
path_data_.SetFrenetPath(path);
}
virtual void SetUp() {
FLAGS_test_data_dir = "modules/planning/testdata/dp_st_speed_test";
}
protected:
const ReferenceLine* reference_line_ = nullptr;
common::TrajectoryPoint init_point_;
SpeedData speed_data_; // output
PathData path_data_; // input
};
TEST_F(DpStSpeedTest, dp_st_graph_test) {
PlanningTestBase::SetUp();
RUN_GOLDEN_TEST;
DpStSpeedConfig dp_st_speed_config;
const auto& veh_param =
common::VehicleConfigHelper::GetConfig().vehicle_param();
DpStGraph dp_st_graph(dp_st_speed_config, StGraphData(), veh_param,
path_data_);
}
} // namespace planning
} // namespace apollo
TMAIN;
......@@ -30,9 +30,9 @@ cc_library(
"//modules/planning/common/path:path_data",
"//modules/planning/common/speed:speed_data",
"//modules/planning/math:polynomial_xd",
"//modules/planning/math/frame_conversion:cartesian_frenet_conversion",
"//modules/planning/math/curve1d:polynomial_curve1d",
"//modules/planning/math/curve1d:quintic_polynomial_curve1d",
"//modules/planning/math/frame_conversion:cartesian_frenet_conversion",
"//modules/planning/math/smoothing_spline:spline_1d_constraint",
"//modules/planning/math/smoothing_spline:spline_1d_generator",
"//modules/planning/math/smoothing_spline:spline_1d_kernel",
......@@ -44,19 +44,4 @@ cc_library(
],
)
cc_test(
name = "qp_spline_path_optimizer_test",
size = "small",
srcs = [
"qp_spline_path_optimizer_test.cc",
],
data = [
"//modules/planning:planning_testdata",
],
deps = [
":qp_spline_path",
"@gtest//:main",
],
)
cpplint()
/******************************************************************************
* 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 <string>
#include <utility>
#include "gtest/gtest.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/optimizer/qp_spline_path/qp_spline_path_optimizer.h"
#include "modules/common/util/file.h"
#include "modules/planning/optimizer/qp_spline_path/qp_spline_path_generator.h"
#include "modules/common/vehicle_state/vehicle_state.h"
namespace apollo {
namespace planning {
using PlanningPb = planning::ADCTrajectory;
class QpSplinePathOptimizerTest : public ::testing::Test {
virtual void SetUp() {
FLAGS_planning_config_file =
"modules/planning/testdata/qp_spline_path/planning_config.pb.txt";
optimizer_.reset(new QpSplinePathOptimizer(""));
frame.reset(new Frame(1));
hdmap::PncMap* pnc_map;
pnc_map = new hdmap::PncMap("modules/planning/testdata/base_map.txt");
Frame::SetMap(pnc_map);
LoadPlanningConfig();
planning_pb = LoadPlanningPb(
"modules/planning/testdata"
"/qp_spline_path/42_apollo_planning.pb.txt");
frame->SetVehicleInitPose(
planning_pb.debug().planning_data().adc_position().pose());
frame->SetRoutingResponse(
planning_pb.debug().planning_data().routing());
}
protected:
PlanningPb LoadPlanningPb(const std::string &filename) {
PlanningPb planning_pb;
CHECK(apollo::common::util::GetProtoFromFile(filename, &planning_pb))
<< "Failed to open file " << filename;
return std::move(planning_pb);
}
void LoadPlanningConfig() {
PlanningPb planning_pb;
CHECK(apollo::common::util::GetProtoFromFile(FLAGS_planning_config_file,
&planning_config_))
<< "Failed to open file " << FLAGS_planning_config_file;
}
common::TrajectoryPoint GetInitPoint() {
common::TrajectoryPoint init_point;
common::VehicleState::instance()->Update(
planning_pb.debug().planning_data().adc_position(),
planning_pb.debug().planning_data().chassis());
init_point.mutable_path_point()->set_x(
common::VehicleState::instance()->x());
init_point.mutable_path_point()->set_y(
common::VehicleState::instance()->y());
init_point.set_v(common::VehicleState::instance()->linear_velocity());
init_point.set_a(common::VehicleState::instance()->linear_acceleration());
init_point.mutable_path_point()->set_theta(
common::VehicleState::instance()->heading());
init_point.mutable_path_point()->set_kappa(
common::VehicleState::instance()->kappa());
init_point.set_relative_time(0.0);
return std::move(init_point);
}
double timestamp_ = 0.0;
std::unique_ptr<Frame> frame;
PlanningPb planning_pb;
std::unique_ptr<Optimizer> optimizer_;
PlanningConfig planning_config_;
};
TEST_F(QpSplinePathOptimizerTest, Process) {
EXPECT_EQ(frame->Init(planning_config_), true);
common::TrajectoryPoint init_point = GetInitPoint();
frame->SetPlanningStartPoint(init_point);
PlanningData* planning_data = frame->mutable_planning_data();
EXPECT_EQ(planning_data->path_data().discretized_path().path_points().size()
, 0);
optimizer_->Init(planning_config_);
optimizer_->Optimize(frame.get());
common::Path qp_path_ground_truth;
for (common::Path path : planning_pb.debug().planning_data().path()) {
if (path.name() == "QP_SPLINE_PATH_OPTIMIZER") {
qp_path_ground_truth = path;
}
}
EXPECT_EQ(qp_path_ground_truth.path_point().size(), 101);
EXPECT_EQ(frame->planning_data().path_data()
.discretized_path().path_points().size(), 101);
for (std::int32_t i = 0; i < qp_path_ground_truth.path_point().size(); i++) {
common::PathPoint ground_truth_point =
qp_path_ground_truth.path_point().Get(i);
common::PathPoint computed_point = planning_data
->path_data().discretized_path().path_points()[i];
EXPECT_NEAR(ground_truth_point.x(), computed_point.x(), 1.0e-2);
EXPECT_NEAR(ground_truth_point.y(), computed_point.y(), 1.0e-3);
EXPECT_NEAR(ground_truth_point.kappa(), computed_point.kappa(), 1.0e-3);
EXPECT_NEAR(ground_truth_point.theta(), computed_point.theta(), 1.0e-3);
EXPECT_NEAR(ground_truth_point.s(), computed_point.s(), 1.0e-3);
EXPECT_NEAR(ground_truth_point.dkappa(), computed_point.dkappa(), 1.0e-3);
EXPECT_NEAR(ground_truth_point.ddkappa(), computed_point.ddkappa(), 1.0e-3);
}
}
} // namespace planning
} // namespace apollo
......@@ -46,22 +46,4 @@ cc_library(
],
)
cc_test(
name = "qp_spline_st_speed_optimizer_test",
size = "small",
srcs = [
"qp_spline_st_speed_optimizer_test.cc",
],
data = [
"//modules/planning:planning_testdata",
],
deps = [
"@gtest//:main",
":qp_spline_st_graph",
":qp_spline_st_speed_optimizer",
"//modules/planning/optimizer/qp_spline_path",
"//modules/planning/proto:qp_spline_path_config_proto",
],
)
cpplint()
/******************************************************************************
* 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 <string>
#include <utility>
#include "gtest/gtest.h"
#include "modules/planning/common/planning_gflags.h"
#include "modules/planning/optimizer/qp_spline_path/qp_spline_path_optimizer.h"
#include "modules/common/util/file.h"
#include "modules/planning/optimizer/qp_spline_path/qp_spline_path_generator.h"
#include "modules/common/vehicle_state/vehicle_state.h"
#include "modules/planning/optimizer/qp_spline_st_speed/qp_spline_st_speed_optimizer.h"
namespace apollo {
namespace planning {
using PlanningPb = planning::ADCTrajectory;
class QpSplineStSpeedOptimizerTest : public ::testing::Test {
virtual void SetUp() {
FLAGS_planning_config_file =
"modules/planning/testdata/qp_spline_st_speed/planning_config.pb.txt";
LoadPlanningConfig();
optimizer_.reset(new QpSplineStSpeedOptimizer("ST_SPEED"));
planning_pb = LoadPlanningPb(
"modules/planning/testdata"
"/qp_spline_st_speed/25_apollo_planning.pb.txt");
InitFrame();
}
protected:
PlanningPb LoadPlanningPb(const std::string &filename) {
PlanningPb planning_pb;
CHECK(apollo::common::util::GetProtoFromFile(filename, &planning_pb))
<< "Failed to open file " << filename;
return std::move(planning_pb);
}
void LoadPlanningConfig() {
PlanningPb planning_pb;
CHECK(apollo::common::util::GetProtoFromFile(FLAGS_planning_config_file,
&planning_config_))
<< "Failed to open file " << FLAGS_planning_config_file;
}
void InitFrame() {
frame.reset(new Frame(1));
hdmap::PncMap* pnc_map;
pnc_map = new hdmap::PncMap("modules/planning/testdata/base_map.txt");
Frame::SetMap(pnc_map);
frame->SetVehicleInitPose(
planning_pb.debug().planning_data().adc_position().pose());
frame->SetRoutingResponse(
planning_pb.debug().planning_data().routing());
frame->Init(planning_config_);
frame->SetPlanningStartPoint(planning_pb.debug()
.planning_data().init_point());
PathData* pathData = frame->mutable_planning_data()->mutable_path_data();
pathData->SetReferenceLine(&frame->reference_line());
pathData->SetDiscretizedPath(getDiscretizedPath());
}
DiscretizedPath getDiscretizedPath() {
DiscretizedPath path;
common::Path qp_path_ground_truth;
for (common::Path path : planning_pb.debug().planning_data().path()) {
if (path.name() == "QP_SPLINE_PATH_OPTIMIZER") {
qp_path_ground_truth = path;
}
}
std::vector<common::PathPoint> pathPoints;
for (common::PathPoint point : qp_path_ground_truth.path_point()) {
pathPoints.push_back(point);
}
path.set_path_points(pathPoints);
return path;
}
double timestamp_ = 0.0;
std::unique_ptr<Frame> frame;
PlanningPb planning_pb;
std::unique_ptr<Optimizer> optimizer_;
PlanningConfig planning_config_;
};
TEST_F(QpSplineStSpeedOptimizerTest, Process) {
PlanningData* planning_data = frame->mutable_planning_data();
EXPECT_EQ(planning_data->speed_data().speed_vector().size()
, 0);
optimizer_->Init(planning_config_);
optimizer_->Optimize(frame.get());
planning_internal::STGraphDebug st_graph_ground_truth;
for (planning_internal::STGraphDebug stGraphDebug :
planning_pb.debug().planning_data().st_graph()) {
if (stGraphDebug.name() == "QP_SPLINE_ST_SPEED_OPTIMIZER") {
st_graph_ground_truth = stGraphDebug;
}
}
EXPECT_EQ(st_graph_ground_truth.speed_profile().size(), 162);
EXPECT_EQ(frame->planning_data().speed_data().speed_vector()
.size(), 162);
for (std::int32_t i = 0;
i < st_graph_ground_truth.speed_profile().size(); i++) {
common::SpeedPoint ground_truth_point =
st_graph_ground_truth.speed_profile().Get(i);
common::SpeedPoint computed_point = planning_data
->speed_data().speed_vector()[i];
// EXPECT_NEAR(ground_truth_point.s(), computed_point.s(), 1.0e-3);
// EXPECT_NEAR(ground_truth_point.v(), computed_point.v(), 1.0e-3);
}
}
} // namespace planning
} // namespace apollo
planner_type : EM
em_planner_config {
optimizer : DP_POLY_PATH_OPTIMIZER
optimizer : DP_ST_SPEED_OPTIMIZER
optimizer : QP_SPLINE_PATH_OPTIMIZER
optimizer : QP_SPLINE_ST_SPEED_OPTIMIZER
dp_poly_path_config {
sample_level: 8
sample_points_num_each_level: 9
step_length_max: 15.0
step_length_min: 8.0
lateral_sample_offset: 0.5
lateral_adjust_coeff: 0.5
eval_time_interval: 0.1
path_resolution: 0.1
}
dp_st_speed_config {
total_path_length: 80
total_time: 8.0
matrix_dimension_s: 200
matrix_dimension_t: 20
speed_weight: 0.0
accel_weight: 10.0
jerk_weight: 10.0
obstacle_weight: 1.0
reference_weight: 0.0
go_down_buffer: 5.0
go_up_buffer: 5.0
default_obstacle_cost: 1e10
obstacle_cost_factor: -300
default_speed_cost: 1.0
exceed_speed_penalty: 10.0
low_speed_penalty: 2.5
accel_penalty: 2.0
decel_penalty: 2.0
positive_jerk_coeff: 1.0
negative_jerk_coeff: 300.0
max_speed: 20.0
max_acceleration: 4.5
max_deceleration: -4.5
st_boundary_config {
boundary_buffer: 0.1
minimal_follow_time: 3.0
follow_buffer: 1.0
follow_coeff:1.0
follow_speed_threshold: 10.0
follow_speed_damping_factor: 0.7
success_tunnel:0.01
expanding_coeff: 1.2
follow_distance_buffer: 5.0
centric_acceleration_limit: 1.0
kappa_threshold: 0.04
minimal_kappa: 0.00001
maximal_speed: 25.0
speed_limit_on_u_turn: 1.5
point_extension: 1.0
speed_multiply_buffer: 0.9
lowest_speed: 2.5
}
}
qp_spline_path_config {
spline_order: 6
number_of_knots: 5
number_of_fx_constraint_knots: 13
time_resolution: 0.1
regularization_weight: 0.1
derivative_weight: 1.0
second_derivative_weight: 0.0
third_derivative_weight: 10.0
reference_line_weight: 0.0
num_refline_point: 10
num_output: 100
}
qp_spline_st_speed_config {
total_path_length: 80.0
total_time: 8.0
output_time_resolution: 0.05
number_of_discrete_graph_s: 80
number_of_discrete_graph_t: 10
number_of_evaluated_graph_t: 20
spline_order: 6
speed_kernel_weight: 0.0
accel_kernel_weight: 0.0
jerk_kernel_weight: 1000.0
follow_weight: 1.0
stop_weight: 1.0
cruise_weight: 1.0
max_speed: 20.0
max_acceleration: 4.5
max_deceleration: -4.5
reference_line_kernel_weight: 1.0
st_boundary_config {
boundary_buffer: 0.1
minimal_follow_time: 3.0
follow_buffer: 1.0
follow_coeff:1.0
follow_speed_threshold: 10.0
follow_speed_damping_factor: 0.7
success_tunnel:0.01
expanding_coeff: 1.2
follow_distance_buffer: 5.0
centric_acceleration_limit: 1.0
kappa_threshold: 0.04
minimal_kappa: 0.00001
maximal_speed: 25.0
speed_limit_on_u_turn: 1.5
point_extension: 1.0
speed_multiply_buffer: 0.9
lowest_speed: 2.5
}
}
}
spline_order: 6
number_of_knots: 5
number_of_fx_constraint_knots: 13
time_resolution: 0.1
regularization_weight: 0.001
derivative_weight: 1.0
second_derivative_weight: 0.0
third_derivative_weight: 0.5
reference_line_weight: 0.0
num_refline_point: 10
num_output: 100
planner_type : EM
em_planner_config {
optimizer : DP_POLY_PATH_OPTIMIZER
optimizer : DP_ST_SPEED_OPTIMIZER
optimizer : QP_SPLINE_PATH_OPTIMIZER
optimizer : QP_SPLINE_ST_SPEED_OPTIMIZER
dp_poly_path_config {
sample_level: 8
sample_points_num_each_level: 9
step_length_max: 15.0
step_length_min: 8.0
lateral_sample_offset: 0.5
lateral_adjust_coeff: 0.5
eval_time_interval: 0.1
path_resolution: 0.1
}
dp_st_speed_config {
total_path_length: 80
total_time: 8.0
matrix_dimension_s: 200
matrix_dimension_t: 20
speed_weight: 0.0
accel_weight: 10.0
jerk_weight: 10.0
obstacle_weight: 1.0
reference_weight: 0.0
go_down_buffer: 5.0
go_up_buffer: 5.0
default_obstacle_cost: 1e10
obstacle_cost_factor: -300
default_speed_cost: 1.0
exceed_speed_penalty: 10.0
low_speed_penalty: 2.5
accel_penalty: 2.0
decel_penalty: 2.0
positive_jerk_coeff: 1.0
negative_jerk_coeff: 300.0
max_speed: 20.0
max_acceleration: 4.5
max_deceleration: -4.5
st_boundary_config {
boundary_buffer: 0.1
minimal_follow_time: 3.0
follow_buffer: 1.0
follow_coeff:1.0
follow_speed_threshold: 10.0
follow_speed_damping_factor: 0.7
success_tunnel:0.01
expanding_coeff: 1.2
follow_distance_buffer: 5.0
centric_acceleration_limit: 1.0
kappa_threshold: 0.04
minimal_kappa: 0.00001
maximal_speed: 25.0
speed_limit_on_u_turn: 1.5
point_extension: 1.0
speed_multiply_buffer: 0.9
lowest_speed: 2.5
}
}
qp_spline_path_config {
spline_order: 6
number_of_knots: 5
number_of_fx_constraint_knots: 13
time_resolution: 0.1
regularization_weight: 0.1
derivative_weight: 1.0
second_derivative_weight: 0.0
third_derivative_weight: 10.0
reference_line_weight: 0.0
num_refline_point: 10
num_output: 100
}
qp_spline_st_speed_config {
total_path_length: 80.0
total_time: 8.0
output_time_resolution: 0.05
number_of_discrete_graph_s: 80
number_of_discrete_graph_t: 10
number_of_evaluated_graph_t: 20
spline_order: 6
speed_kernel_weight: 0.0
accel_kernel_weight: 0.0
jerk_kernel_weight: 1000.0
follow_weight: 1.0
stop_weight: 1.0
cruise_weight: 1.0
max_speed: 20.0
max_acceleration: 4.5
max_deceleration: -4.5
reference_line_kernel_weight: 1.0
st_boundary_config {
boundary_buffer: 0.1
minimal_follow_time: 3.0
follow_buffer: 1.0
follow_coeff:1.0
follow_speed_threshold: 10.0
follow_speed_damping_factor: 0.7
success_tunnel:0.01
expanding_coeff: 1.2
follow_distance_buffer: 5.0
centric_acceleration_limit: 1.0
kappa_threshold: 0.04
minimal_kappa: 0.00001
maximal_speed: 25.0
speed_limit_on_u_turn: 1.5
point_extension: 1.0
speed_multiply_buffer: 0.9
lowest_speed: 2.5
}
}
}
total_path_length: 80.0
total_time: 8.0
output_time_resolution: 0.05
number_of_discrete_graph_s: 80
number_of_discrete_graph_t: 10
number_of_evaluated_graph_t: 20
spline_order: 6
speed_kernel_weight: 0.0
accel_kernel_weight: 0.0
jerk_kernel_weight: 1000.0
follow_weight: 0.2
stop_weight: 0.2
cruise_weight: 0.2
max_speed: 20.0
max_acceleration: 2.5
max_deceleration: -4.5
reference_line_kernel_weight: 1.0
boundary_buffer: 0.1
minimal_follow_time: 3.0
follow_buffer: 1.0
follow_coeff:1.0
follow_speed_threshold: 10.0
follow_speed_damping_factor: 0.7
success_tunnel:0.01
expanding_coeff: 1.2
follow_distance_buffer: 5.0
centric_acceleration_limit: 1.0
kappa_threshold: 0.04
minimal_kappa: 0.00001
maximal_speed: 25.0
speed_limit_on_u_turn: 1.5
point_extension: 1.0
speed_multiply_buffer: 0.9
lowest_speed: 2.5
Markdown is supported
0% .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑!
想要评论请 注册