Skip to content

Commit

Permalink
refactor(path_shifter): make member functions const as far as possible (
Browse files Browse the repository at this point in the history 
autowarefoundation#1875)

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
  • Loading branch information
@satoshi-ota
satoshi-ota committed Dec 15, 2022
1 parent 18b7c0d commit 858be7d
Show file tree
Hide file tree
Showing 2 changed files with 73 additions and 61 deletions.
46 changes: 29 additions & 17 deletions ...g/behavior_path_planner/include/behavior_path_planner/scene_module/utils/path_shifter.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,19 +83,33 @@ class PathShifter
*/
void setShiftPoints(const std::vector<ShiftPoint> & points);

/**
* @brief Get shift points.
*/
std::vector<ShiftPoint> getShiftPoints() const { return shift_points_; }
PathWithLaneId getReferencePath() const { return reference_path_; }

/**
* @brief Get shift points size.
*/
size_t getShiftPointsSize() const { return shift_points_.size(); }

/**
* @brief Get base offset.
*/
double getBaseOffset() const { return base_offset_; }

/**
* @brief Get reference path.
*/
PathWithLaneId getReferencePath() const { return reference_path_; }

/**
* @brief Generate a shifted path according to the given reference path and shift points.
* @return False if the path is empty or shift points have conflicts.
*/
bool generate(
ShiftedPath * shift_path, const bool offset_back = true,
const SHIFT_TYPE type = SHIFT_TYPE::SPLINE);
ShiftedPath * shifted_path, const bool offset_back = true,
const SHIFT_TYPE type = SHIFT_TYPE::SPLINE) const;

/**
* @brief Remove behind shift points and add the removed offset to the base_offset_.
Expand Down Expand Up @@ -145,10 +159,6 @@ class PathShifter
return base_offset_;
}

// TODO(Horibe) enable this with const
// if (!is_index_aligned_) {
// updateShiftPointIndices();
// }
const auto furthest = std::max_element(
shift_points_.begin(), shift_points_.end(),
[](auto & a, auto & b) { return a.end_idx < b.end_idx; });
Expand All @@ -173,7 +183,7 @@ class PathShifter
* @brief Calculate the theoretical lateral jerk by spline shifting for current shift_points_.
* @return Jerk array. THe size is same as the shift points.
*/
std::vector<double> calcLateralJerk();
std::vector<double> calcLateralJerk() const;

/**
* @brief Calculate shift point from path arclength for start and end point.
Expand All @@ -192,33 +202,35 @@ class PathShifter
// The amount of shift length to the entire path.
double base_offset_{0.0};

// Flag to check the path index is aligned. (cleared when new path or shift points are received)
bool is_index_aligned_{false};
// Logger
mutable rclcpp::Logger logger_{
rclcpp::get_logger("behavior_path_planner").get_child("path_shifter")};

rclcpp::Logger logger_{rclcpp::get_logger("behavior_path_planner").get_child("path_shifter")};
// Clock
mutable rclcpp::Clock clock_{RCL_ROS_TIME};

/**
* @brief Calculate path index for shift_points and set is_index_aligned_ to true.
*/
void updateShiftPointIndices();
void updateShiftPointIndices(ShiftPointArray & shift_points) const;

/**
* @brief Sort the points in order from the front of the path.
*/
bool sortShiftPointsAlongPath(const PathWithLaneId & path);
void sortShiftPointsAlongPath(ShiftPointArray & shift_points) const;

/**
* @brief Generate shifted path from reference_path_ and shift_points_ with linear shifting.
*/
void applyLinearShifter(ShiftedPath * shifted_path);
void applyLinearShifter(ShiftedPath * shifted_path) const;

/**
* @brief Generate shifted path from reference_path_ and shift_points_ with spline_based shifting.
* @details Calculate the shift so that the horizontal jerk remains constant. This is achieved by
* dividing the shift interval into four parts and apply a cubic spline to them.
* The resultant shifting shape is closed to the Clothoid curve.
*/
void applySplineShifter(ShiftedPath * shifted_path, const bool offset_back);
void applySplineShifter(ShiftedPath * shifted_path, const bool offset_back) const;

////////////////////////////////////////
// Helper Functions
Expand All @@ -229,9 +241,9 @@ class PathShifter
*/
bool checkShiftPointsAlignment(const ShiftPointArray & shift_points) const;

void addLateralOffsetOnIndexPoint(ShiftedPath * point, double offset, size_t index) const;
void addLateralOffsetOnIndexPoint(ShiftedPath * path, double offset, size_t index) const;

void shiftBaseLength(ShiftedPath * point, double offset) const;
void shiftBaseLength(ShiftedPath * path, double offset) const;

void setBaseOffset(const double val)
{
Expand Down
88 changes: 44 additions & 44 deletions planning/behavior_path_planner/src/scene_module/utils/path_shifter.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -43,25 +43,34 @@ std::string toStr(const behavior_path_planner::ShiftPoint & p)

namespace behavior_path_planner
{

using motion_utils::findNearestIndex;

void PathShifter::setPath(const PathWithLaneId & path)
{
reference_path_ = path;
is_index_aligned_ = false; // shift_point index has to be updated for new path.

updateShiftPointIndices(shift_points_);
sortShiftPointsAlongPath(shift_points_);
}
void PathShifter::addShiftPoint(const ShiftPoint & point)
{
shift_points_.push_back(point);
is_index_aligned_ = false; // shift_point index has to be updated for new shift points.

updateShiftPointIndices(shift_points_);
sortShiftPointsAlongPath(shift_points_);
}

void PathShifter::setShiftPoints(const std::vector<ShiftPoint> & points)
{
shift_points_ = points;
is_index_aligned_ = false; // shift_point index has to be updated for new shift points.

updateShiftPointIndices(shift_points_);
sortShiftPointsAlongPath(shift_points_);
}

bool PathShifter::generate(
ShiftedPath * shifted_path, const bool offset_back, const SHIFT_TYPE type)
ShiftedPath * shifted_path, const bool offset_back, const SHIFT_TYPE type) const
{
RCLCPP_DEBUG_STREAM(logger_, "PathShifter::generate start!");

Expand All @@ -80,23 +89,20 @@ bool PathShifter::generate(
return true;
}

if (!is_index_aligned_) {
updateShiftPointIndices();
for (const auto & shift_point : shift_points_) {
int idx_gap = shift_point.end_idx - shift_point.start_idx;
if (idx_gap <= 1) {
RCLCPP_WARN_STREAM(
logger_,
"shift start point and end point can't be adjoining "
"Maybe shift length is too short?");
return false;
}
for (const auto & shift_point : shift_points_) {
int idx_gap = shift_point.end_idx - shift_point.start_idx;
if (idx_gap <= 1) {
RCLCPP_WARN_STREAM_THROTTLE(
logger_, clock_, 3000,
"shift start point and end point can't be adjoining "
"Maybe shift length is too short?");
return false;
}
}

// Sort shift points since applyShift function only supports sorted points
if (!sortShiftPointsAlongPath(reference_path_)) {
RCLCPP_ERROR_STREAM(logger_, "has duplicated points. Failed!");
// Check if the shift points are sorted correctly
if (!checkShiftPointsAlignment(shift_points_)) {
RCLCPP_ERROR_STREAM(logger_, "Failed to sort shift points..!!");
return false;
}

Expand All @@ -121,7 +127,7 @@ bool PathShifter::generate(
return true;
}

void PathShifter::applyLinearShifter(ShiftedPath * shifted_path)
void PathShifter::applyLinearShifter(ShiftedPath * shifted_path) const
{
const auto arclength_arr = util::calcPathArcLengthArray(reference_path_);

Expand Down Expand Up @@ -155,7 +161,7 @@ void PathShifter::applyLinearShifter(ShiftedPath * shifted_path)
}
}

void PathShifter::applySplineShifter(ShiftedPath * shifted_path, const bool offset_back)
void PathShifter::applySplineShifter(ShiftedPath * shifted_path, const bool offset_back) const
{
const auto arclength_arr = util::calcPathArcLengthArray(reference_path_);

Expand Down Expand Up @@ -224,13 +230,13 @@ void PathShifter::applySplineShifter(ShiftedPath * shifted_path, const bool offs
}
}

std::vector<double> PathShifter::calcLateralJerk()
std::vector<double> PathShifter::calcLateralJerk() const
{
const auto arclength_arr = util::calcPathArcLengthArray(reference_path_);

constexpr double epsilon = 1.0e-8; // to avoid 0 division

std::vector<double> lateral_jerk;
std::vector<double> lateral_jerk{};

// TODO(Watanabe) write docs.
double current_shift = base_offset_;
Expand Down Expand Up @@ -308,13 +314,16 @@ bool PathShifter::calcShiftPointFromArcLength(
return true;
}

void PathShifter::updateShiftPointIndices()
void PathShifter::updateShiftPointIndices(ShiftPointArray & shift_points) const
{
for (auto & p : shift_points_) {
p.start_idx = motion_utils::findNearestIndex(reference_path_.points, p.start.position);
p.end_idx = motion_utils::findNearestIndex(reference_path_.points, p.end.position);
for (auto & p : shift_points) {
// TODO(murooka) remove findNearestIndex for except
// lane_following to support u-turn & crossing path
p.start_idx = findNearestIndex(reference_path_.points, p.start.position);
// TODO(murooka) remove findNearestIndex for except
// lane_following to support u-turn & crossing path
p.end_idx = findNearestIndex(reference_path_.points, p.end.position);
}
is_index_aligned_ = true;
}

bool PathShifter::checkShiftPointsAlignment(const ShiftPointArray & shift_points) const
Expand All @@ -333,20 +342,20 @@ bool PathShifter::checkShiftPointsAlignment(const ShiftPointArray & shift_points
return true;
}

bool PathShifter::sortShiftPointsAlongPath([[maybe_unused]] const PathWithLaneId & path)
void PathShifter::sortShiftPointsAlongPath(ShiftPointArray & shift_points) const
{
if (shift_points_.empty()) {
RCLCPP_DEBUG_STREAM(logger_, "shift_points_ is empty. do nothing.");
return true;
if (shift_points.empty()) {
RCLCPP_DEBUG_STREAM_THROTTLE(logger_, clock_, 3000, "shift_points is empty. do nothing.");
return;
}

const auto & unsorted_shift_points = shift_points_;
const auto unsorted_shift_points = shift_points;

// Calc indices sorted by "shift start point index" order
std::vector<size_t> sorted_indices(unsorted_shift_points.size());
std::iota(sorted_indices.begin(), sorted_indices.end(), 0);
std::sort(sorted_indices.begin(), sorted_indices.end(), [&](size_t i1, size_t i2) {
return unsorted_shift_points[i1].start_idx < unsorted_shift_points[i2].start_idx;
std::sort(sorted_indices.begin(), sorted_indices.end(), [&](size_t i, size_t j) {
return unsorted_shift_points.at(i).start_idx < unsorted_shift_points.at(j).start_idx;
});

// Set shift points and index by sorted_indices
Expand All @@ -355,14 +364,7 @@ bool PathShifter::sortShiftPointsAlongPath([[maybe_unused]] const PathWithLaneId
sorted_shift_points.push_back(unsorted_shift_points.at(sorted_i));
}

// Check if the shift points are sorted correctly
if (!checkShiftPointsAlignment(sorted_shift_points)) {
RCLCPP_ERROR_STREAM(logger_, "Failed to sort shift points..!!");
return false;
}

// set to member
setShiftPoints(sorted_shift_points);
shift_points = sorted_shift_points;

// Debug
for (const auto & p : unsorted_shift_points) {
Expand All @@ -375,8 +377,6 @@ bool PathShifter::sortShiftPointsAlongPath([[maybe_unused]] const PathWithLaneId
RCLCPP_DEBUG_STREAM(logger_, "sorted_shift_points: in order: " << toStr(p));
}
RCLCPP_DEBUG(logger_, "PathShifter::sortShiftPointsAlongPath end.");

return true;
}

void PathShifter::removeBehindShiftPointAndSetBaseOffset(const Point & base_point)
Expand Down

0 comments on commit 858be7d

Please sign in to comment.