diff --git a/modules/planning/common/planning_gflags.cc b/modules/planning/common/planning_gflags.cc index fe63f35ef6f21259fad8c6fbcac4fafa769fb92a..1a396ae65e6970bc6891e744eab37699c9841606 100644 --- a/modules/planning/common/planning_gflags.cc +++ b/modules/planning/common/planning_gflags.cc @@ -20,8 +20,7 @@ DEFINE_int32(planning_loop_rate, 5, "Loop rate for planning node"); DEFINE_string(rtk_trajectory_filename, "modules/planning/data/garage.csv", "Loop rate for planning node"); -DEFINE_string(map_filename, "modules/map/data/base_map.txt", - "map data file"); +DEFINE_string(map_filename, "modules/map/data/base_map.txt", "map data file"); DEFINE_uint64(rtk_trajectory_backward, 10, "The number of points to be included in RTK trajectory " @@ -91,6 +90,8 @@ DEFINE_double(longitudinal_jerk_lower_bound, -4.0, DEFINE_double(longitudinal_jerk_upper_bound, 4.0, "The upper bound of longitudinal jerk."); +DEFINE_double(kappa_bound, 0.23, "The bound for vehicle curvature"); + DEFINE_double(stgraph_default_point_cost, 1e10, "The default stgraph point cost."); DEFINE_double(stgraph_max_acceleration_divide_factor_level_1, 2.0, diff --git a/modules/planning/common/planning_gflags.h b/modules/planning/common/planning_gflags.h index 99ea527288cf3e6cc05d40ecd1122df4fb92a9cd..1c4613478709a36820e2c5052ceb2cb33d38b7dd 100644 --- a/modules/planning/common/planning_gflags.h +++ b/modules/planning/common/planning_gflags.h @@ -70,6 +70,8 @@ DECLARE_double(lateral_jerk_bound); DECLARE_double(longitudinal_jerk_lower_bound); DECLARE_double(longitudinal_jerk_upper_bound); +DECLARE_double(kappa_bound); + // STGraph DECLARE_double(stgraph_default_point_cost); DECLARE_double(stgraph_max_acceleration_divide_factor_level_1); diff --git a/modules/planning/trajectory_stitcher/BUILD b/modules/planning/trajectory_stitcher/BUILD index 23a4867fe3303a7bf76d42ee251ea75927c5fb5a..1cf72203e1f38b96acd3cf3956dee81509b3c420 100644 --- a/modules/planning/trajectory_stitcher/BUILD +++ b/modules/planning/trajectory_stitcher/BUILD @@ -19,4 +19,22 @@ cc_library( ], ) +cc_library( + name = "constraint_checker", + srcs = [ + "constraint_checker.cc", + ], + hdrs = [ + "constraint_checker.h", + ], + deps = [ + "//modules/common:log", + "//modules/common/proto:path_point_proto", + "//modules/common/vehicle_state", + "//modules/planning/common:planning_gflags", + "//modules/planning/common/trajectory", + "//modules/planning/common/trajectory:discretized_trajectory", + ], +) + cpplint() diff --git a/modules/planning/trajectory_stitcher/constraint_checker.cc b/modules/planning/trajectory_stitcher/constraint_checker.cc new file mode 100644 index 0000000000000000000000000000000000000000..d18b0b7d038ca50c7080aee3409ae0c41d98f029 --- /dev/null +++ b/modules/planning/trajectory_stitcher/constraint_checker.cc @@ -0,0 +1,115 @@ +/****************************************************************************** + * 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. + *****************************************************************************/ + +/** + * @file constraint_checker.cc + **/ + +#include "modules/planning/trajectory_stitcher/constraint_checker.h" + +#include "modules/common/log.h" +#include "modules/planning/common/planning_gflags.h" + +namespace apollo { +namespace planning { + +namespace { +template +bool within_range(const T v, const T lower, const T upper) { + if (v < lower || v > upper) { + return false; + } + return true; +} +} + +bool ConstraintChecker::valid_trajectory( + const DiscretizedTrajectory& trajectory) { + for (const auto& p : trajectory.trajectory_points()) { + double t = p.relative_time(); + double lon_v = p.v(); + if (!within_range(lon_v, FLAGS_speed_lower_bound, + FLAGS_speed_upper_bound)) { + AWARN << "Velocity at relative time " << t + << " exceeds bound, value: " << lon_v << ", bound [" + << FLAGS_speed_lower_bound << ", " << FLAGS_speed_upper_bound + << "]."; + return false; + } + + double lon_a = p.a(); + if (!within_range(lon_a, FLAGS_longitudinal_acceleration_lower_bound, + FLAGS_longitudinal_acceleration_upper_bound)) { + AWARN << "Longitudinal acceleration at relative time " << t + << " exceeds bound, value: " << lon_a << ", bound [" + << FLAGS_longitudinal_acceleration_lower_bound << ", " + << FLAGS_longitudinal_acceleration_upper_bound << "]."; + return false; + } + + double lat_a = lon_v * lon_v * p.path_point().kappa(); + if (!within_range(lat_a, -FLAGS_lateral_acceleration_bound, + FLAGS_lateral_acceleration_bound)) { + AWARN << "Lateral acceleration at relative time " << t + << " exceeds bound, value: " << lat_a << ", bound [" + << -FLAGS_lateral_acceleration_bound << ", " + << FLAGS_lateral_acceleration_bound << "]."; + return false; + } + + double kappa = p.path_point().kappa(); + if (!within_range(kappa, -FLAGS_kappa_bound, FLAGS_kappa_bound)) { + AWARN << "Kappa at relative time " << t + << " exceeds bound, value: " << kappa << ", bound [" + << -FLAGS_kappa_bound << ", " << FLAGS_kappa_bound << "]."; + return false; + } + } + + for (std::size_t i = 1; i < trajectory.num_of_points(); ++i) { + const auto& p0 = trajectory.trajectory_point_at(i - 1); + const auto& p1 = trajectory.trajectory_point_at(i); + double t = p0.relative_time(); + + double dt = p1.relative_time() - p0.relative_time(); + double d_lon_a = p1.a() - p0.a(); + double lon_jerk = d_lon_a / dt; + if (!within_range(d_lon_a / dt, FLAGS_longitudinal_jerk_lower_bound, + FLAGS_longitudinal_jerk_upper_bound)) { + AWARN << "Longitudinal jerk at relative time " << t + << " exceeds bound, value: " << lon_jerk << ", bound [" + << FLAGS_longitudinal_jerk_lower_bound << ", " + << FLAGS_longitudinal_jerk_upper_bound << "]."; + return false; + } + + double d_lat_a = p1.v() * p1.v() * p1.path_point().kappa() - + p0.v() * p0.v() * p0.path_point().kappa(); + double lat_jerk = d_lat_a / dt; + if (!within_range(lat_jerk, -FLAGS_lateral_jerk_bound, + FLAGS_lateral_jerk_bound)) { + AWARN << "Lateral jerk at relative time " << t + << " exceeds bound, value: " << lat_jerk << ", bound [" + << -FLAGS_lateral_jerk_bound << ", " << FLAGS_lateral_jerk_bound + << "]."; + return false; + } + } + return true; +} + +} // namespace planning +} // namespace apollo diff --git a/modules/planning/trajectory_stitcher/constraint_checker.h b/modules/planning/trajectory_stitcher/constraint_checker.h new file mode 100644 index 0000000000000000000000000000000000000000..5192223de66aa14d368b9c4a246d50866665e1c1 --- /dev/null +++ b/modules/planning/trajectory_stitcher/constraint_checker.h @@ -0,0 +1,38 @@ +/****************************************************************************** + * 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. + *****************************************************************************/ + +/** + * @file constraint_checker.h + **/ + +#ifndef MODULES_PLANNING_CONSTRAINT_CHECKER_H_ +#define MODULES_PLANNING_CONSTRAINT_CHECKER_H_ + +#include "modules/planning/common/trajectory/discretized_trajectory.h" + +namespace apollo { +namespace planning { + +class ConstraintChecker { + public: + ConstraintChecker() = delete; + static bool valid_trajectory(const DiscretizedTrajectory& trajectory); +}; + +} // namespace planning +} // namespace apollo + +#endif // MODULES_PLANNING_CONSTRAINT_CHECKER_H_