diff --git a/modules/common/math/math_utils.cc b/modules/common/math/math_utils.cc index dd70cab168f7ce84d3ce7700f5514b75c09bc919..24ba75f59eda7a5420a1ff77ed6fefecbf83a7d5 100644 --- a/modules/common/math/math_utils.cc +++ b/modules/common/math/math_utils.cc @@ -65,7 +65,7 @@ double RandomDouble(const double s, const double t, unsigned int rand_seed) { return s + (t - s) / 16383.0 * (rand_r(&rand_seed) & 16383); } -int double_compare(const double d1, const double d2, const double epsilon) { +int DoubleCompare(const double d1, const double d2, const double epsilon) { DCHECK(!std::isnan(d1)); DCHECK(!std::isnan(d2)); diff --git a/modules/common/math/math_utils.h b/modules/common/math/math_utils.h index e03df2a6be4222d345adcd1fa27acf8d776c7c1a..8ae38bb95a2e8b2460f8e21de46b4a7ef9bb7462 100644 --- a/modules/common/math/math_utils.h +++ b/modules/common/math/math_utils.h @@ -152,7 +152,7 @@ T Clamp(const T value, T bound1, T bound2) { return value; } -int double_compare( +int DoubleCompare( const double d1, const double d2, const double epsilon = std::numeric_limits::epsilon()); diff --git a/modules/common/math/math_utils_test.cc b/modules/common/math/math_utils_test.cc index f6527cf982085938b25fc93d15ce79fdedca0fb9..7643ab232a15b4061341bd6263bcce415a7ae140 100644 --- a/modules/common/math/math_utils_test.cc +++ b/modules/common/math/math_utils_test.cc @@ -73,16 +73,16 @@ TEST(MathUtilsTest, Square) { } TEST(MathUtilsTest, Double) { - EXPECT_EQ(double_compare(234.32, 93.9), 1); - EXPECT_EQ(double_compare({234.32}, {93.9}), 1); - EXPECT_EQ(double_compare(234.32, 93.9, 1e-5), 1); - EXPECT_EQ(double_compare({234.32}, {93.9}, 1e-5), 1); + EXPECT_EQ(DoubleCompare(234.32, 93.9), 1); + EXPECT_EQ(DoubleCompare({234.32}, {93.9}), 1); + EXPECT_EQ(DoubleCompare(234.32, 93.9, 1e-5), 1); + EXPECT_EQ(DoubleCompare({234.32}, {93.9}, 1e-5), 1); - EXPECT_EQ(double_compare(23.32, 93.9), -1); - EXPECT_EQ(double_compare(4.32, 4.32), 0); - EXPECT_EQ(double_compare(2.1, 2.0009, 1e-5), 1); - EXPECT_EQ(double_compare(1.1, 2.0009, 1e-5), -1); - EXPECT_EQ(double_compare(2.1, 2.0009, 1), 0); + EXPECT_EQ(DoubleCompare(23.32, 93.9), -1); + EXPECT_EQ(DoubleCompare(4.32, 4.32), 0); + EXPECT_EQ(DoubleCompare(2.1, 2.0009, 1e-5), 1); + EXPECT_EQ(DoubleCompare(1.1, 2.0009, 1e-5), -1); + EXPECT_EQ(DoubleCompare(2.1, 2.0009, 1), 0); } } // namespace math diff --git a/modules/control/common/trajectory_analyzer.cc b/modules/control/common/trajectory_analyzer.cc index 705594daf2c608038a86c220910a01a6d86f0ce1..3a56cb7acf48c3c406ebb6b11fe5f5dd702b494c 100644 --- a/modules/control/common/trajectory_analyzer.cc +++ b/modules/control/common/trajectory_analyzer.cc @@ -81,8 +81,8 @@ PathPoint TrajectoryAnalyzer::QueryMatchedPathPoint(const double x, index_min + 1 == trajectory_points_.size() ? index_min : index_min + 1; if (index_start == index_end || - common::math::double_compare(trajectory_points_[index_start].s, - trajectory_points_[index_end].s) == 0) { + common::math::DoubleCompare(trajectory_points_[index_start].s, + trajectory_points_[index_end].s) == 0) { return trajectory_points_[index_start]; } diff --git a/modules/prediction/common/prediction_gflags.cc b/modules/prediction/common/prediction_gflags.cc index aac3faf8c4d3ebee6c4d29fd7ed8f75b1b79e8d5..ff6a7dbc3fcd2ed339fd7b4c41a95a291dee7f59 100644 --- a/modules/prediction/common/prediction_gflags.cc +++ b/modules/prediction/common/prediction_gflags.cc @@ -21,3 +21,8 @@ DEFINE_string(prediction_module_name, "prediction", "Default prediciton module name"); DEFINE_string(prediction_conf_file, "modules/prediction/conf/prediction_conf.pb.txt", "Default conf file for prediction"); + +// Features +DEFINE_double(double_precision, 1e-6, "precision of double"); +DEFINE_double(max_acc, 4.0, "Upper bound of acceleration"); +DEFINE_double(min_acc, -4.0, "Lower bound of deceleration"); diff --git a/modules/prediction/common/prediction_gflags.h b/modules/prediction/common/prediction_gflags.h index 1e8c17705a706b4154de3dcc21c9d3977670e88b..6b45a54db3f68678096e8b3af5156199f8f382f8 100644 --- a/modules/prediction/common/prediction_gflags.h +++ b/modules/prediction/common/prediction_gflags.h @@ -23,4 +23,8 @@ DECLARE_string(prediction_module_name); DECLARE_string(prediction_conf_file); +DECLARE_double(double_precision); +DECLARE_double(max_acc); +DECLARE_double(min_acc); + #endif // MODULES_PREDICTION_COMMON_PREDICTION_GFLAGS_H_ diff --git a/modules/prediction/container/obstacles/BUILD b/modules/prediction/container/obstacles/BUILD index b633e32ab4932762f88c4e4461b4df6c0e485ea0..d01c8829ada0104daf82fea8cf29c44d3f739629 100644 --- a/modules/prediction/container/obstacles/BUILD +++ b/modules/prediction/container/obstacles/BUILD @@ -15,10 +15,12 @@ cc_library( srcs = ["obstacle.cc"], hdrs = ["obstacle.h"], deps = [ + "//modules/common:log", + "//modules/common/math:kalman_filter", "//modules/common/proto:error_code_proto", + "//modules/common/math:math_utils", "//modules/perception/proto:perception_proto", "//modules/prediction/proto:feature_proto", - "//modules/common:log", - "//modules/common/math:kalman_filter", + "//modules/prediction/common:prediction_common", ], -) \ No newline at end of file +) diff --git a/modules/prediction/container/obstacles/obstacle.cc b/modules/prediction/container/obstacles/obstacle.cc index 10b201097035a651be5a93687b60b7f57af49e83..c43900b1c8522ba4fa9210c4c358b703e8681e13 100644 --- a/modules/prediction/container/obstacles/obstacle.cc +++ b/modules/prediction/container/obstacles/obstacle.cc @@ -20,6 +20,8 @@ #include #include "modules/common/log.h" +#include "modules/common/math/math_utils.h" +#include "modules/prediction/common/prediction_gflags.h" namespace apollo { namespace prediction { @@ -256,8 +258,7 @@ void Obstacle::SetAcceleration(Feature* feature) { const Point3D& curr_velocity = feature->velocity(); const Point3D& prev_velocity = feature_history_.front().velocity(); - // TODO(kechxu) add a gflag of double precision - if (curr_ts > prev_ts) { + if (apollo::common::math::DoubleCompare(curr_ts, prev_ts) == 1) { double damping_x = Damp(curr_velocity.x(), 0.001); double damping_y = Damp(curr_velocity.y(), 0.001); double damping_z = Damp(curr_velocity.z(), 0.001); @@ -265,10 +266,13 @@ void Obstacle::SetAcceleration(Feature* feature) { acc_x = (curr_velocity.x() - prev_velocity.x()) / (curr_ts - prev_ts); acc_y = (curr_velocity.y() - prev_velocity.y()) / (curr_ts - prev_ts); acc_z = (curr_velocity.z() - prev_velocity.z()) / (curr_ts - prev_ts); - // TODO(kechxu) clamp the acc - acc_x *= damping_x; - acc_y *= damping_y; - acc_z *= damping_z; + + acc_x = apollo::common::math::Clamp( + acc_x * damping_x, FLAGS_min_acc, FLAGS_max_acc); + acc_y = apollo::common::math::Clamp( + acc_y * damping_y, FLAGS_min_acc, FLAGS_max_acc); + acc_z = apollo::common::math::Clamp( + acc_z * damping_z, FLAGS_min_acc, FLAGS_max_acc); } }