提交 77f4a449 编写于 作者: Z Zhang Liangliang 提交者: Jiangtao Hu

ALL: fixed all lint errors.

上级 b7244a7c
...@@ -18,7 +18,7 @@ ...@@ -18,7 +18,7 @@
#include <algorithm> #include <algorithm>
#include <boost/filesystem.hpp> #include "boost/filesystem.hpp"
namespace apollo { namespace apollo {
namespace perception { namespace perception {
......
...@@ -15,7 +15,7 @@ ...@@ -15,7 +15,7 @@
*****************************************************************************/ *****************************************************************************/
/** /**
* @file trjactory_cost.h * @file
**/ **/
#include "modules/planning/tasks/dp_poly_path/trajectory_cost.h" #include "modules/planning/tasks/dp_poly_path/trajectory_cost.h"
...@@ -35,15 +35,14 @@ using apollo::common::math::Vec2d; ...@@ -35,15 +35,14 @@ using apollo::common::math::Vec2d;
using apollo::common::TrajectoryPoint; using apollo::common::TrajectoryPoint;
TrajectoryCost::TrajectoryCost( TrajectoryCost::TrajectoryCost(
const DpPolyPathConfig &config, const DpPolyPathConfig &config, const ReferenceLine &reference_line,
const ReferenceLine &reference_line, const std::vector<const PathObstacle *> &obstacles,
const std::vector<const PathObstacle*>& obstacles,
const common::VehicleParam &vehicle_param, const common::VehicleParam &vehicle_param,
const SpeedData &heuristic_speed_data) : const SpeedData &heuristic_speed_data)
config_(config), : config_(config),
reference_line_(&reference_line), reference_line_(&reference_line),
vehicle_param_(vehicle_param), vehicle_param_(vehicle_param),
heuristic_speed_data_(heuristic_speed_data) { heuristic_speed_data_(heuristic_speed_data) {
const double total_time = const double total_time =
std::min(heuristic_speed_data_.TotalTime(), FLAGS_prediction_total_time); std::min(heuristic_speed_data_.TotalTime(), FLAGS_prediction_total_time);
...@@ -56,13 +55,13 @@ TrajectoryCost::TrajectoryCost( ...@@ -56,13 +55,13 @@ TrajectoryCost::TrajectoryCost(
} }
const auto ptr_obstacle = ptr_path_obstacle->obstacle(); const auto ptr_obstacle = ptr_path_obstacle->obstacle();
if (ptr_obstacle->PerceptionId() < 0) { if (ptr_obstacle->PerceptionId() < 0) {
//Virtual obsatcle // Virtual obsatcle
continue; continue;
} }
std::vector<Box2d> box_by_time; std::vector<Box2d> box_by_time;
for (uint32_t t = 0; t <= num_of_time_stamps_; ++t) { for (uint32_t t = 0; t <= num_of_time_stamps_; ++t) {
TrajectoryPoint trajectory_point = TrajectoryPoint trajectory_point =
ptr_obstacle->GetPointAtTime(t * config.eval_time_interval()); ptr_obstacle->GetPointAtTime(t * config.eval_time_interval());
Box2d obs_box = ptr_obstacle->GetBoundingBox(trajectory_point); Box2d obs_box = ptr_obstacle->GetBoundingBox(trajectory_point);
box_by_time.push_back(obs_box); box_by_time.push_back(obs_box);
} }
...@@ -77,15 +76,15 @@ double TrajectoryCost::Calculate(const QuinticPolynomialCurve1d &curve, ...@@ -77,15 +76,15 @@ double TrajectoryCost::Calculate(const QuinticPolynomialCurve1d &curve,
// path_cost // path_cost
double path_s = 0.0; double path_s = 0.0;
while (path_s < (end_s - start_s)) { while (path_s < (end_s - start_s)) {
double l = std::fabs(curve.Evaluate(0, path_s)); const double l = std::fabs(curve.Evaluate(0, path_s));
total_cost += l; total_cost += l;
double dl = std::fabs(curve.Evaluate(1, path_s)); const double dl = std::fabs(curve.Evaluate(1, path_s));
total_cost += dl; total_cost += dl;
path_s += config_.path_resolution(); path_s += config_.path_resolution();
} }
// obstacle cost // obstacle cost
uint32_t start_index = 0; uint32_t start_index = 0;
bool is_found_start_index = false; bool is_found_start_index = false;
...@@ -109,24 +108,24 @@ double TrajectoryCost::Calculate(const QuinticPolynomialCurve1d &curve, ...@@ -109,24 +108,24 @@ double TrajectoryCost::Calculate(const QuinticPolynomialCurve1d &curve,
for (; start_index < end_index; ++start_index) { for (; start_index < end_index; ++start_index) {
common::SpeedPoint speed_point; common::SpeedPoint speed_point;
heuristic_speed_data_.EvaluateByTime( heuristic_speed_data_.EvaluateByTime(
start_index * config_.eval_time_interval(), &speed_point); start_index * config_.eval_time_interval(), &speed_point);
double s = speed_point.s() - start_s; const double s = speed_point.s() - start_s;
double l = curve.Evaluate(0, s); const double l = curve.Evaluate(0, s);
double dl = curve.Evaluate(1, s); const double dl = curve.Evaluate(1, s);
Vec2d ego_xy_point; Vec2d ego_xy_point;
common::SLPoint sl; common::SLPoint sl;
sl.set_s(speed_point.s()); sl.set_s(speed_point.s());
sl.set_l(l); sl.set_l(l);
reference_line_->SLToXY(sl, &ego_xy_point); reference_line_->SLToXY(sl, &ego_xy_point);
ReferencePoint reference_point = ReferencePoint reference_point =
reference_line_->GetReferencePoint(speed_point.s()); reference_line_->GetReferencePoint(speed_point.s());
double one_minus_kappa_r_d = 1 - reference_point.kappa() * l; double one_minus_kappa_r_d = 1 - reference_point.kappa() * l;
double delta_theta = std::atan2(dl, one_minus_kappa_r_d); double delta_theta = std::atan2(dl, one_minus_kappa_r_d);
double theta = common::math::NormalizeAngle( double theta =
delta_theta + reference_point.heading()); common::math::NormalizeAngle(delta_theta + reference_point.heading());
Box2d ego_box = {ego_xy_point, theta, Box2d ego_box = {ego_xy_point, theta, vehicle_param_.length(),
vehicle_param_.length(), vehicle_param_.width()}; vehicle_param_.width()};
for (const auto &obstacle_trajectory : obstacle_boxes_) { for (const auto &obstacle_trajectory : obstacle_boxes_) {
auto &obstacle_box = obstacle_trajectory[start_index]; auto &obstacle_box = obstacle_trajectory[start_index];
double distance = obstacle_box.DistanceTo(ego_box); double distance = obstacle_box.DistanceTo(ego_box);
......
...@@ -18,8 +18,8 @@ ...@@ -18,8 +18,8 @@
* @file trjactory_cost.h * @file trjactory_cost.h
**/ **/
#ifndef MODULES_PLANNING_TASKS_DP_POLY_PATH_TRAJECTORY_COST_H #ifndef MODULES_PLANNING_TASKS_DP_POLY_PATH_TRAJECTORY_COST_H_
#define MODULES_PLANNING_TASKS_DP_POLY_PATH_TRAJECTORY_COST_H #define MODULES_PLANNING_TASKS_DP_POLY_PATH_TRAJECTORY_COST_H_
#include <vector> #include <vector>
...@@ -40,7 +40,7 @@ class TrajectoryCost { ...@@ -40,7 +40,7 @@ class TrajectoryCost {
public: public:
explicit TrajectoryCost(const DpPolyPathConfig &config, explicit TrajectoryCost(const DpPolyPathConfig &config,
const ReferenceLine &reference_line, const ReferenceLine &reference_line,
const std::vector<const PathObstacle*>& obstacles, const std::vector<const PathObstacle *> &obstacles,
const common::VehicleParam &vehicle_param, const common::VehicleParam &vehicle_param,
const SpeedData &heuristic_speed_data); const SpeedData &heuristic_speed_data);
double Calculate(const QuinticPolynomialCurve1d &curve, const double start_s, double Calculate(const QuinticPolynomialCurve1d &curve, const double start_s,
...@@ -51,7 +51,7 @@ class TrajectoryCost { ...@@ -51,7 +51,7 @@ class TrajectoryCost {
const ReferenceLine *reference_line_ = nullptr; const ReferenceLine *reference_line_ = nullptr;
const common::VehicleParam vehicle_param_; const common::VehicleParam vehicle_param_;
SpeedData heuristic_speed_data_; SpeedData heuristic_speed_data_;
uint32_t num_of_time_stamps_; uint32_t num_of_time_stamps_ = 0;
std::vector<std::vector<common::math::Box2d>> obstacle_boxes_; std::vector<std::vector<common::math::Box2d>> obstacle_boxes_;
std::vector<double> obstacle_probabilities_; std::vector<double> obstacle_probabilities_;
}; };
...@@ -59,4 +59,4 @@ class TrajectoryCost { ...@@ -59,4 +59,4 @@ class TrajectoryCost {
} // namespace planning } // namespace planning
} // namespace apollo } // namespace apollo
#endif // MODULES_PLANNING_TASKS_DP_POLY_PATH_TRAJECTORY_COST_H #endif // MODULES_PLANNING_TASKS_DP_POLY_PATH_TRAJECTORY_COST_H_
...@@ -22,6 +22,7 @@ ...@@ -22,6 +22,7 @@
#define MODULES_PLANNING_TASKS_QP_SPLINE_ST_SPEED_QP_SPLINE_ST_GRAPH_H_ #define MODULES_PLANNING_TASKS_QP_SPLINE_ST_SPEED_QP_SPLINE_ST_GRAPH_H_
#include <memory> #include <memory>
#include <utility>
#include <vector> #include <vector>
#include "modules/common/configs/proto/vehicle_config.pb.h" #include "modules/common/configs/proto/vehicle_config.pb.h"
......
...@@ -21,6 +21,7 @@ ...@@ -21,6 +21,7 @@
#include "modules/planning/tasks/qp_spline_st_speed/qp_spline_st_speed_optimizer.h" #include "modules/planning/tasks/qp_spline_st_speed/qp_spline_st_speed_optimizer.h"
#include <algorithm> #include <algorithm>
#include <utility>
#include <vector> #include <vector>
#include "modules/common/proto/pnc_point.pb.h" #include "modules/common/proto/pnc_point.pb.h"
......
...@@ -33,7 +33,7 @@ class PoseContainerTest : public ::testing::Test { ...@@ -33,7 +33,7 @@ class PoseContainerTest : public ::testing::Test {
PoseContainerTest() = default; PoseContainerTest() = default;
virtual ~PoseContainerTest() = default; virtual ~PoseContainerTest() = default;
virtual void SetUp() override {} void SetUp() override {}
protected: protected:
void InitPose(LocalizationEstimate *localization); void InitPose(LocalizationEstimate *localization);
......
...@@ -28,4 +28,4 @@ DEFINE_bool(use_road_id, true, "enable use road id to cut routing result"); ...@@ -28,4 +28,4 @@ DEFINE_bool(use_road_id, true, "enable use road id to cut routing result");
DEFINE_double(min_length_for_lane_change, 10.0, DEFINE_double(min_length_for_lane_change, 10.0,
"min length for lane change, in creater, in meter"); "min length for lane change, in creater, in meter");
DEFINE_bool(enable_change_lane_in_result, false, DEFINE_bool(enable_change_lane_in_result, false,
"contain change lane operator in result"); "contain change lane operator in result");
\ No newline at end of file
Markdown is supported
0% .
You are about to add 0 people to the discussion. Proceed with caution.
先完成此消息的编辑!
想要评论请 注册