12
12
// See the License for the specific language governing permissions and
13
13
// limitations under the License.
14
14
15
- #include \
16
- " static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp"
15
+ #include " static_centerline_generator/centerline_source/optimization_trajectory_based_centerline.hpp"
17
16
18
17
#include " motion_utils/resample/resample.hpp"
19
18
#include " motion_utils/trajectory/conversion.hpp"
@@ -91,19 +90,19 @@ OptimizationTrajectoryBasedCenterline::generate_centerline_with_optimization(
91
90
92
91
// extract path with lane id from lanelets
93
92
const auto raw_path_with_lane_id = [&]() {
94
- const auto non_resampled_path_with_lane_id = utils::get_path_with_lane_id (
95
- route_handler, route_lanelets, start_center_pose, ego_nearest_dist_threshold,
96
- ego_nearest_yaw_threshold);
97
- return motion_utils::resamplePath (non_resampled_path_with_lane_id, behavior_path_interval);
98
- }();
93
+ const auto non_resampled_path_with_lane_id = utils::get_path_with_lane_id (
94
+ route_handler, route_lanelets, start_center_pose, ego_nearest_dist_threshold,
95
+ ego_nearest_yaw_threshold);
96
+ return motion_utils::resamplePath (non_resampled_path_with_lane_id, behavior_path_interval);
97
+ }();
99
98
pub_raw_path_with_lane_id_->publish (raw_path_with_lane_id);
100
99
RCLCPP_INFO (node.get_logger (), " Calculated raw path with lane id and published." );
101
100
102
101
// convert path with lane id to path
103
102
const auto raw_path = [&]() {
104
- const auto non_resampled_path = convert_to_path (raw_path_with_lane_id);
105
- return motion_utils::resamplePath (non_resampled_path, behavior_vel_interval);
106
- }();
103
+ const auto non_resampled_path = convert_to_path (raw_path_with_lane_id);
104
+ return motion_utils::resamplePath (non_resampled_path, behavior_vel_interval);
105
+ }();
107
106
pub_raw_path_->publish (raw_path);
108
107
RCLCPP_INFO (node.get_logger (), " Converted to path and published." );
109
108
@@ -121,9 +120,9 @@ std::vector<TrajectoryPoint> OptimizationTrajectoryBasedCenterline::optimize_tra
121
120
{
122
121
// convert to trajectory points
123
122
const auto raw_traj_points = [&]() {
124
- const auto raw_traj = convert_to_trajectory (raw_path);
125
- return motion_utils::convertToTrajectoryPointArray (raw_traj);
126
- }();
123
+ const auto raw_traj = convert_to_trajectory (raw_path);
124
+ return motion_utils::convertToTrajectoryPointArray (raw_traj);
125
+ }();
127
126
128
127
// create an instance of elastic band and model predictive trajectory.
129
128
const auto eb_path_smoother_ptr =
@@ -133,25 +132,24 @@ std::vector<TrajectoryPoint> OptimizationTrajectoryBasedCenterline::optimize_tra
133
132
134
133
// NOTE: The optimization is executed every valid_optimized_traj_points_num points.
135
134
constexpr int valid_optimized_traj_points_num = 10 ;
136
- const int traj_segment_num = static_cast < int >(raw_traj_points. size ()) /
137
- valid_optimized_traj_points_num;
135
+ const int traj_segment_num =
136
+ static_cast < int >(raw_traj_points. size ()) / valid_optimized_traj_points_num;
138
137
139
138
// NOTE: num_initial_optimization exists to make the both optimizations stable since they may use
140
139
// warm start.
141
140
constexpr int num_initial_optimization = 2 ;
142
141
143
142
std::vector<TrajectoryPoint> whole_optimized_traj_points;
144
143
for (int virtual_ego_pose_idx = -num_initial_optimization;
145
- virtual_ego_pose_idx < traj_segment_num; ++virtual_ego_pose_idx)
146
- {
144
+ virtual_ego_pose_idx < traj_segment_num; ++virtual_ego_pose_idx) {
147
145
// calculate virtual ego pose for the optimization
148
146
constexpr int virtual_ego_pose_offset_idx = 1 ;
149
147
const auto virtual_ego_pose =
150
148
raw_traj_points
151
- .at (
152
- valid_optimized_traj_points_num * std::max (virtual_ego_pose_idx, 0 ) +
153
- virtual_ego_pose_offset_idx)
154
- .pose ;
149
+ .at (
150
+ valid_optimized_traj_points_num * std::max (virtual_ego_pose_idx, 0 ) +
151
+ virtual_ego_pose_offset_idx)
152
+ .pose ;
155
153
156
154
// smooth trajectory by elastic band in the path_smoother package
157
155
const auto smoothed_traj_points =
0 commit comments