Skip to content

Commit

Permalink
finally
Browse files Browse the repository at this point in the history
Signed-off-by: Zulfaqar Azmi <[email protected]>
  • Loading branch information
zulfaqar-azmi-t4 committed Nov 30, 2023
1 parent 8ee4168 commit bc99260
Show file tree
Hide file tree
Showing 3 changed files with 6 additions and 6 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -94,8 +94,8 @@ std::pair<bool, int32_t> ControlPerformanceAnalysisCore::findClosestPrevWayPoint
return std::make_pair(false, std::numeric_limits<int32_t>::quiet_NaN());
}

idx_prev_wp_ = std::make_unique<int32_t>(closest_segment.get());
idx_next_wp_ = std::make_unique<int32_t>(closest_segment.get() + 1);
idx_prev_wp_ = std::make_unique<int32_t>(closest_segment.value());
idx_next_wp_ = std::make_unique<int32_t>(closest_segment.value() + 1);
return std::make_pair(true, *idx_prev_wp_);
}

Expand Down Expand Up @@ -429,7 +429,7 @@ double ControlPerformanceAnalysisCore::estimateCurvature()
RCLCPP_WARN(logger_, "Cannot find index of curvature reference waypoint ");
return 0;
}
const auto idx_curve_ref_wp = idx_curve_ref_wp_optional.get();
const auto idx_curve_ref_wp = idx_curve_ref_wp_optional.value();

const auto & points = current_trajectory_ptr_->points;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -43,7 +43,7 @@ class ElasticBandSmoother : public rclcpp::Node
bool isDrivingForward(const std::vector<PathPoint> & path_points)
{
const auto is_driving_forward = motion_utils::isDrivingForward(path_points);
is_driving_forward_ = is_driving_forward ? is_driving_forward.get() : is_driving_forward_;
is_driving_forward_ = is_driving_forward ? is_driving_forward.value() : is_driving_forward_;
return is_driving_forward_;
}

Expand Down
4 changes: 2 additions & 2 deletions planning/path_smoother/src/elastic_band_smoother.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -55,7 +55,7 @@ void setZeroVelocityAfterStopPoint(std::vector<TrajectoryPoint> & traj_points)
{
const auto opt_zero_vel_idx = motion_utils::searchZeroVelocityIndex(traj_points);
if (opt_zero_vel_idx) {
for (size_t i = opt_zero_vel_idx.get(); i < traj_points.size(); ++i) {
for (size_t i = opt_zero_vel_idx.value(); i < traj_points.size(); ++i) {
traj_points.at(i).longitudinal_velocity_mps = 0.0;
}
}
Expand Down Expand Up @@ -284,7 +284,7 @@ void ElasticBandSmoother::applyInputVelocity(
// insert stop point explicitly
const auto stop_idx = motion_utils::searchZeroVelocityIndex(forward_cropped_input_traj_points);
if (stop_idx) {
const auto & input_stop_pose = forward_cropped_input_traj_points.at(stop_idx.get()).pose;
const auto & input_stop_pose = forward_cropped_input_traj_points.at(stop_idx.value()).pose;
// NOTE: motion_utils::findNearestSegmentIndex is used instead of
// trajectory_utils::findEgoSegmentIndex
// for the case where input_traj_points is much longer than output_traj_points, and the
Expand Down

0 comments on commit bc99260

Please sign in to comment.