@@ -66,7 +66,7 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
66
66
ego_data.pose = planner_data_->current_odometry ->pose ;
67
67
ego_data.path .points = path->points ;
68
68
ego_data.first_path_idx =
69
- motion_utils::findNearestSegmentIndex (ego_data. path . points , ego_data.pose .position );
69
+ motion_utils::findNearestSegmentIndex (path-> points , ego_data.pose .position );
70
70
motion_utils::removeOverlapPoints (ego_data.path .points );
71
71
ego_data.velocity = planner_data_->current_velocity ->twist .linear .x ;
72
72
ego_data.max_decel = -planner_data_->max_stop_acceleration_threshold ;
@@ -75,13 +75,13 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
75
75
const auto path_footprints = calculate_path_footprints (ego_data, params_);
76
76
const auto calculate_path_footprints_us = stopwatch.toc (" calculate_path_footprints" );
77
77
// Calculate lanelets to ignore and consider
78
- const auto path_lanelets = planning_utils::getLaneletsOnPath (
79
- ego_data.path , planner_data_->route_handler_ ->getLaneletMapPtr (),
80
- planner_data_->current_odometry ->pose );
78
+ stopwatch.tic (" calculate_lanelets" );
79
+ const auto path_lanelets = calculate_path_lanelets (ego_data, *planner_data_->route_handler_ );
81
80
const auto ignored_lanelets =
82
81
calculate_ignored_lanelets (ego_data, path_lanelets, *planner_data_->route_handler_ , params_);
83
82
const auto other_lanelets = calculate_other_lanelets (
84
83
ego_data, path_lanelets, ignored_lanelets, *planner_data_->route_handler_ , params_);
84
+ const auto calculate_lanelets_us = stopwatch.toc (" calculate_lanelets" );
85
85
86
86
debug_data_.footprints = path_footprints;
87
87
debug_data_.path_lanelets = path_lanelets;
@@ -98,6 +98,7 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
98
98
});
99
99
if (overlapped_lanelet_it != other_lanelets.end ()) {
100
100
debug_data_.current_overlapped_lanelets .push_back (*overlapped_lanelet_it);
101
+ // TODO(Maxime): we may want to just add the overlapped lane to the ignored lanelets
101
102
RCLCPP_DEBUG (logger_, " Ego is already overlapping a lane, skipping the module ()\n " );
102
103
return true ;
103
104
}
@@ -183,13 +184,15 @@ bool OutOfLaneModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
183
184
RCLCPP_DEBUG (
184
185
logger_,
185
186
" Total time = %2.2fus\n "
187
+ " \t calculate_lanelets = %2.0fus\n "
186
188
" \t calculate_path_footprints = %2.0fus\n "
187
189
" \t calculate_overlapping_ranges = %2.0fus\n "
188
190
" \t calculate_decisions = %2.0fus\n "
189
191
" \t calc_slowdown_points = %2.0fus\n "
190
192
" \t insert_slowdown_points = %2.0fus\n " ,
191
- total_time_us, calculate_path_footprints_us, calculate_overlapping_ranges_us,
192
- calculate_decisions_us, calc_slowdown_points_us, insert_slowdown_points_us);
193
+ total_time_us, calculate_lanelets_us, calculate_path_footprints_us,
194
+ calculate_overlapping_ranges_us, calculate_decisions_us, calc_slowdown_points_us,
195
+ insert_slowdown_points_us);
193
196
return true ;
194
197
}
195
198
0 commit comments