File tree 3 files changed +7
-5
lines changed
planning/autoware_path_optimizer
include/autoware/path_optimizer
3 files changed +7
-5
lines changed Original file line number Diff line number Diff line change 18
18
#include " autoware/path_optimizer/common_structs.hpp"
19
19
#include " autoware/path_optimizer/state_equation_generator.hpp"
20
20
#include " autoware/path_optimizer/type_alias.hpp"
21
+ #include " autoware/path_optimizer/utils/conditional_timer.hpp"
21
22
#include " autoware/universe_utils/geometry/geometry.hpp"
22
23
#include " autoware/universe_utils/system/time_keeper.hpp"
23
24
#include " autoware_vehicle_info_utils/vehicle_info_utils.hpp"
26
27
#include " interpolation/spline_interpolation_points_2d.hpp"
27
28
#include " osqp_interface/osqp_interface.hpp"
28
29
29
- #include " autoware/path_optimizer/utils/conditional_timer.hpp"
30
-
31
30
#include < Eigen/Core>
32
31
#include < Eigen/Sparse>
33
32
Original file line number Diff line number Diff line change @@ -483,7 +483,8 @@ void MPTOptimizer::onParam(const std::vector<rclcpp::Parameter> & parameters)
483
483
debug_data_ptr_->mpt_visualize_sampling_num = mpt_param_.mpt_visualize_sampling_num ;
484
484
}
485
485
486
- std::pair<std::vector<TrajectoryPoint>, int > MPTOptimizer::optimizeTrajectory (const PlannerData & planner_data)
486
+ std::pair<std::vector<TrajectoryPoint>, int > MPTOptimizer::optimizeTrajectory (
487
+ const PlannerData & planner_data)
487
488
{
488
489
autoware::universe_utils::ScopedTimeTrack st (__func__, *time_keeper_);
489
490
Original file line number Diff line number Diff line change @@ -139,7 +139,8 @@ PathOptimizer::PathOptimizer(const rclcpp::NodeOptions & node_options)
139
139
// Diagnostics
140
140
{
141
141
updater_.setHardwareID (" path_optimizer" );
142
- updater_.add (" path_optimizer_emergency_stop" , this , &PathOptimizer::onCheckPathOptimizationValid);
142
+ updater_.add (
143
+ " path_optimizer_emergency_stop" , this , &PathOptimizer::onCheckPathOptimizationValid);
143
144
}
144
145
145
146
time_keeper_ =
@@ -313,7 +314,8 @@ void PathOptimizer::onCheckPathOptimizationValid(diagnostic_updater::DiagnosticS
313
314
{
314
315
const std::string error_msg = " [Path Optimizer]: MRM not required" ;
315
316
if (mrm_required_) {
316
- const std::string error_msg = " [Path Optimizer]: Emergency Brake due to prolonged Path Optimization failure" ;
317
+ const std::string error_msg =
318
+ " [Path Optimizer]: Emergency Brake due to prolonged Path Optimization failure" ;
317
319
const auto diag_level = diagnostic_msgs::msg::DiagnosticStatus::ERROR;
318
320
stat.summary (diag_level, error_msg);
319
321
} else {
You can’t perform that action at this time.
0 commit comments