Skip to content

Commit 7302e8c

Browse files
PanConChicharronPanConChicharron
authored
feat(autoware_path_optimizer): reverts new path optimizer due to failing builds (autowarefoundation#12298)
Revert "feat(autoware_path_optimizer): new path optimizer (autowarefoundation#11479)" This reverts commit f775ea6.
1 parent 0a309ac commit 7302e8c

20 files changed

+47
-2281
lines changed

‎.cspell.json‎

Lines changed: 4 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -6,47 +6,13 @@
66
],
77
"ignoreRegExpList": [],
88
"words": [
9-
"blasfeo",
10-
"brkay54",
11-
"CUDA_HOSTDEV",
12-
"epsi",
13-
"expl",
149
"dltype",
15-
"ferror",
16-
"forw",
10+
"tvmgen",
1711
"fromarray",
18-
"hpipm",
19-
"HPIPM",
20-
"idxbu",
21-
"idxsh",
22-
"ineq",
23-
"lambdify",
24-
"Lambdify",
25-
"libtensorrt",
26-
"NBXN",
27-
"NPHI",
28-
"NPHIN",
29-
"NSBX",
30-
"NSBXN",
31-
"NSBU",
32-
"NSGN",
33-
"NSHN",
34-
"NSPHI",
35-
"NSPHIN",
36-
"Psi̇",
37-
"psidot",
38-
"Psidot",
39-
"relim",
4012
"soblin",
41-
"trigsimp",
42-
"tvmgen",
13+
"brkay54",
14+
"libtensorrt",
4315
"TRTBEV",
44-
"utraj",
45-
"vertcat",
46-
"xdot",
47-
"xtraj",
48-
"ydot",
49-
"Ydot",
50-
"yref"
16+
"CUDA_HOSTDEV"
5117
]
5218
}

‎planning/autoware_mission_planner_universe/schema/mission_planner.schema.json‎

Lines changed: 0 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -65,22 +65,6 @@
6565
"type": "boolean",
6666
"description": "This flag is for considering no_drivable_lanes in planning or not",
6767
"default": "false"
68-
},
69-
"mpt": {
70-
"type": "object",
71-
"properties": {
72-
"use_acados": {
73-
"type": "boolean",
74-
"description": "Enable/disable acados solver for MPT optimization",
75-
"default": false
76-
},
77-
"use_acados_circle_constraints": {
78-
"type": "boolean",
79-
"description": "Enable/disable the new MPT-style circle road-bound constraints in acados",
80-
"default": true
81-
}
82-
},
83-
"required": ["use_acados", "use_acados_circle_constraints"]
8468
}
8569
},
8670
"required": [

‎planning/autoware_path_optimizer/CMakeLists.txt‎

Lines changed: 27 additions & 51 deletions
Original file line numberDiff line numberDiff line change
@@ -13,79 +13,55 @@ include(CheckCXXCompilerFlag)
1313
# endif()
1414

1515
find_package(autoware_cmake REQUIRED)
16-
17-
# Exclude generated acados code from copyright checking
18-
# Must be set before autoware_package() is called (when lint hooks are registered)
19-
# Use glob patterns to exclude all generated files (they're created during build)
20-
if(NOT DEFINED AMENT_LINT_AUTO_FILE_EXCLUDE)
21-
set(AMENT_LINT_AUTO_FILE_EXCLUDE "" CACHE INTERNAL "")
22-
endif()
23-
list(APPEND AMENT_LINT_AUTO_FILE_EXCLUDE
24-
"src/acados_mpc/c_generated_code/**"
25-
"src/acados_mpc/c_generated_code/**/*"
26-
"src/acados_mpc/c_generated_code/**/*.*"
27-
)
28-
2916
autoware_package()
3017

3118
find_package(Eigen3 REQUIRED)
32-
find_package(acados REQUIRED)
33-
34-
add_subdirectory(src/acados_mpc)
3519

36-
set(ACADOS_DEPS_INCLUDE_DIRS "")
37-
foreach(_target acados blasfeo hpipm)
38-
get_target_property(_dirs ${_target} INTERFACE_INCLUDE_DIRECTORIES)
39-
if(_dirs)
40-
list(APPEND ACADOS_DEPS_INCLUDE_DIRS ${_dirs})
41-
endif()
42-
endforeach()
43-
list(REMOVE_DUPLICATES ACADOS_DEPS_INCLUDE_DIRS)
44-
if(ACADOS_DEPS_INCLUDE_DIRS)
45-
ament_export_include_directories(${ACADOS_DEPS_INCLUDE_DIRS})
46-
endif()
47-
48-
find_package(ament_cmake_export_dependencies REQUIRED)
49-
ament_export_dependencies(acados)
50-
51-
ament_auto_add_library(${PROJECT_NAME} SHARED
20+
ament_auto_add_library(autoware_path_optimizer SHARED
21+
# node
5222
src/node.cpp
23+
# core algorithms
5324
src/replan_checker.cpp
5425
src/mpt_optimizer.cpp
5526
src/state_equation_generator.cpp
27+
# debug marker
5628
src/debug_marker.cpp
29+
# vehicle model
5730
src/vehicle_model/vehicle_model_interface.cpp
5831
src/vehicle_model/vehicle_model_bicycle_kinematics.cpp
32+
# utils
5933
src/utils/trajectory_utils.cpp
6034
src/utils/geometry_utils.cpp
6135
)
6236

63-
target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${EIGEN3_INCLUDE_DIR})
64-
target_link_libraries(${PROJECT_NAME} acados_interface)
37+
target_include_directories(autoware_path_optimizer
38+
SYSTEM PUBLIC
39+
${EIGEN3_INCLUDE_DIR}
40+
)
6541

66-
rclcpp_components_register_node(${PROJECT_NAME}
42+
# register node
43+
rclcpp_components_register_node(autoware_path_optimizer
6744
PLUGIN "autoware::path_optimizer::PathOptimizer"
6845
EXECUTABLE path_optimizer_node
6946
)
7047

7148
if(BUILD_TESTING)
72-
ament_add_ros_isolated_gtest(test_${PROJECT_NAME} test/test_path_optimizer_node_interface.cpp)
73-
target_link_libraries(test_${PROJECT_NAME} ${PROJECT_NAME})
49+
ament_add_ros_isolated_gtest(test_${PROJECT_NAME}
50+
test/test_path_optimizer_node_interface.cpp
51+
)
52+
target_link_libraries(test_${PROJECT_NAME}
53+
${PROJECT_NAME}
54+
)
7455
endif()
7556

76-
ament_auto_package(INSTALL_TO_SHARE launch config rviz)
77-
78-
install(PROGRAMS scripts/calculation_time_plotter.py DESTINATION lib/${PROJECT_NAME})
79-
install(TARGETS acados_interface LIBRARY DESTINATION lib)
80-
install(FILES src/acados_mpc/include/acados_interface.hpp DESTINATION include/autoware/path_optimizer)
57+
ament_auto_package(
58+
INSTALL_TO_SHARE
59+
launch
60+
config
61+
rviz
62+
)
8163

82-
# Downstream (e.g. autoware_trajectory_optimizer) includes acados_interface.hpp -> c_generated_code/...
83-
set(MODEL "curvilinear_bicycle_model_spatial")
84-
set(GEN_DIR "${CMAKE_CURRENT_SOURCE_DIR}/src/acados_mpc/c_generated_code")
85-
install(FILES
86-
${GEN_DIR}/acados_solver_${MODEL}.h
87-
${GEN_DIR}/${MODEL}_model/${MODEL}_model.h
88-
${GEN_DIR}/${MODEL}_constraints/${MODEL}_constraints.h
89-
DESTINATION include/autoware/path_optimizer/c_generated_code
90-
OPTIONAL
64+
install(PROGRAMS
65+
scripts/calculation_time_plotter.py
66+
DESTINATION lib/${PROJECT_NAME}
9167
)

‎planning/autoware_path_optimizer/config/path_optimizer.param.yaml‎

Lines changed: 0 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -35,10 +35,6 @@
3535

3636
# mpt param
3737
mpt:
38-
use_acados: false
39-
# Enable/disable the new MPT-style circle road-bound constraints in acados.
40-
use_acados_circle_constraints: true
41-
4238
option:
4339
# TODO(murooka) enable the following. Currently enabling the flag makes QP so heavy
4440
steer_limit_constraint: false

‎planning/autoware_path_optimizer/include/autoware/path_optimizer/mpt_optimizer.hpp‎

Lines changed: 2 additions & 61 deletions
Original file line numberDiff line numberDiff line change
@@ -15,7 +15,6 @@
1515
#ifndef AUTOWARE__PATH_OPTIMIZER__MPT_OPTIMIZER_HPP_
1616
#define AUTOWARE__PATH_OPTIMIZER__MPT_OPTIMIZER_HPP_
1717

18-
#include "acados_interface.hpp"
1918
#include "autoware/interpolation/linear_interpolation.hpp"
2019
#include "autoware/interpolation/spline_interpolation_points_2d.hpp"
2120
#include "autoware/osqp_interface/osqp_interface.hpp"
@@ -31,10 +30,6 @@
3130
#include <Eigen/Core>
3231
#include <Eigen/Sparse>
3332

34-
#include <std_msgs/msg/float32_multi_array.hpp>
35-
#include <std_msgs/msg/multi_array_dimension.hpp>
36-
#include <std_msgs/msg/multi_array_layout.hpp>
37-
3833
#include <memory>
3934
#include <optional>
4035
#include <string>
@@ -260,11 +255,6 @@ class MPTOptimizer
260255
double delta_arc_length;
261256
int num_points;
262257

263-
bool use_acados;
264-
// Toggle MPT-style acados road-bound circle constraints (lh/uh on h(x,p)).
265-
// When false, we still widen lh/uh to effectively disable generated constraints.
266-
bool use_acados_circle_constraints;
267-
268258
// kinematics
269259
double optimization_center_offset;
270260
double max_steer_rad;
@@ -322,15 +312,6 @@ class MPTOptimizer
322312
rclcpp::Publisher<Trajectory>::SharedPtr debug_fixed_traj_pub_;
323313
rclcpp::Publisher<Trajectory>::SharedPtr debug_ref_traj_pub_;
324314
rclcpp::Publisher<Trajectory>::SharedPtr debug_mpt_traj_pub_;
325-
rclcpp::Publisher<Trajectory>::SharedPtr debug_acados_mpt_traj_pub_;
326-
327-
// Add new publishers for spline coefficients and curvatures
328-
rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr debug_optimised_steering_pub_;
329-
rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr
330-
debug_acados_optimised_steering_pub_;
331-
rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr debug_optimised_states_pub_;
332-
rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr debug_acados_optimised_states_pub_;
333-
rclcpp::Publisher<std_msgs::msg::Float32MultiArray>::SharedPtr debug_ref_steering_pub_;
334315

335316
// argument
336317
bool enable_debug_info_;
@@ -344,7 +325,6 @@ class MPTOptimizer
344325

345326
StateEquationGenerator state_equation_generator_;
346327
std::unique_ptr<autoware::osqp_interface::OSQPInterface> osqp_solver_ptr_;
347-
AcadosInterface acados_interface_{20, 1e-6};
348328

349329
const double osqp_epsilon_ = 1.0e-3;
350330

@@ -361,9 +341,8 @@ class MPTOptimizer
361341

362342
void updateVehicleCircles();
363343

364-
std::pair<std::vector<ReferencePoint>, autoware::interpolation::SplineInterpolationPoints2d>
365-
calcReferencePoints(
366-
const PlannerData & planner_data, const std::vector<TrajectoryPoint> & smoothed_points);
344+
std::vector<ReferencePoint> calcReferencePoints(
345+
const PlannerData & planner_data, const std::vector<TrajectoryPoint> & smoothed_points) const;
367346
void updateCurvature(
368347
std::vector<ReferencePoint> & ref_points,
369348
const autoware::interpolation::SplineInterpolationPoints2d & ref_points_spline) const;
@@ -421,44 +400,6 @@ class MPTOptimizer
421400
void publishDebugTrajectories(
422401
const std_msgs::msg::Header & header, const std::vector<ReferencePoint> & ref_points,
423402
const std::vector<TrajectoryPoint> & mpt_traj_points) const;
424-
425-
// Helper method to build parameters for Acados solver
426-
std::array<double, NP> buildParameters(
427-
const std::vector<double> & knots, const std::vector<double> & curvatures) const;
428-
429-
// Set parameters on the AcadosInterface for all stages
430-
void setParametersToSolver(
431-
const std::array<double, NP> & parameters, const std::vector<ReferencePoint> & ref_points,
432-
const double s0);
433-
434-
// Convert Acados solution to trajectory points
435-
std::optional<std::vector<TrajectoryPoint>> convertAcadosSolutionToTrajectory(
436-
std::vector<ReferencePoint> & ref_points, const AcadosSolution & acados_solution) const;
437-
438-
AcadosSolution runAcadosMPT(
439-
const std::vector<ReferencePoint> & ref_points,
440-
autoware::interpolation::SplineInterpolationPoints2d & ref_points_spline,
441-
const geometry_msgs::msg::Pose & ego_pose);
442-
443-
void updateDebugDataAndPublishAcadosSteering(
444-
const AcadosSolution & acados_result, const std::vector<ReferencePoint> & ref_points,
445-
const std::vector<TrajectoryPoint> & acados_traj_points);
446-
447-
void publishAcadosStates(const AcadosSolution & acados_result) const;
448-
449-
void publishOptimizedSteering(const Eigen::VectorXd & optimized_variables) const;
450-
void publishOptimizedStates(const Eigen::VectorXd & states, const size_t N) const;
451-
452-
void publishAcadosTrajectory(
453-
const std::vector<TrajectoryPoint> & acados_traj_points,
454-
const std_msgs::msg::Header & header) const;
455-
456-
void publishReferenceTrajectory(
457-
const std::vector<ReferencePoint> & ref_points, const std_msgs::msg::Header & header) const;
458-
459-
void publishReferenceSteeringAngles(
460-
const autoware::interpolation::SplineInterpolationPoints2d & ref_points_spline) const;
461-
462403
std::vector<TrajectoryPoint> extractFixedPoints(
463404
const std::vector<ReferencePoint> & ref_points) const;
464405

‎planning/autoware_path_optimizer/schema/path_optimizer.schema.json‎

Lines changed: 0 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -144,16 +144,6 @@
144144
"mpt": {
145145
"type": "object",
146146
"properties": {
147-
"use_acados": {
148-
"type": "boolean",
149-
"description": "Enable/disable acados solver for MPT optimization",
150-
"default": false
151-
},
152-
"use_acados_circle_constraints": {
153-
"type": "boolean",
154-
"description": "Enable/disable the new MPT-style circle road-bound constraints in acados",
155-
"default": true
156-
},
157147
"option": {
158148
"type": "object",
159149
"properties": {
@@ -487,8 +477,6 @@
487477
}
488478
},
489479
"required": [
490-
"use_acados",
491-
"use_acados_circle_constraints",
492480
"option",
493481
"common",
494482
"clearance",

0 commit comments

Comments
 (0)