Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactor(run_out): use util function to calculate stop point #1780

Merged
merged 3 commits into from
Sep 6, 2022
Merged

refactor(run_out): use util function to calculate stop point #1780

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
9259fbe
refactor: use util function to calculate stop point
TomohitoAndo Sep 5, 2022
397f367
refactor: remove unnecessary debug marker
TomohitoAndo Sep 5, 2022
de4d662
refactor: revert an unrelated change
TomohitoAndo Sep 5, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
40 changes: 0 additions & 40 deletions planning/behavior_velocity_planner/include/scene_module/run_out/path_utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -48,46 +48,6 @@ geometry_msgs::msg::Point findLongitudinalNearestPoint(
return min_dist_point;
}

template <class T>
size_t calcIndexByLength(
const T & points, const geometry_msgs::msg::Pose & current_pose, const double target_length)
{
const size_t nearest_index = motion_utils::findNearestIndex(points, current_pose.position);
if (target_length < 0) {
return nearest_index;
}

for (size_t i = nearest_index; i < points.size(); i++) {
double length_sum = motion_utils::calcSignedArcLength(points, current_pose.position, i);
if (length_sum > target_length) {
return i;
}
}

// reach the end of the points, so return the last index
return points.size() - 1;
}

template <class T>
size_t calcIndexByLengthReverse(
const T & points, const geometry_msgs::msg::Point & src_point, const float target_length)
{
const auto nearest_seg_idx = motion_utils::findNearestSegmentIndex(points, src_point);
if (nearest_seg_idx == 0) {
return 0;
}

for (size_t i = nearest_seg_idx; i > 0; i--) {
const auto length_sum = std::abs(motion_utils::calcSignedArcLength(points, src_point, i));
if (length_sum > target_length) {
return i + 1;
}
}

// reach the beginning of the path
return 0;
}

} // namespace run_out_utils
} // namespace behavior_velocity_planner
#endif // SCENE_MODULE__RUN_OUT__PATH_UTILS_HPP_
5 changes: 2 additions & 3 deletions planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -124,12 +124,11 @@ class RunOutModule : public SceneModuleInterface
void insertStoppingVelocity(
const boost::optional<DynamicObstacle> & dynamic_obstacle,
const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc,
const PathWithLaneId & smoothed_path, PathWithLaneId & output_path);
PathWithLaneId & output_path);

void insertApproachingVelocity(
const DynamicObstacle & dynamic_obstacle, const geometry_msgs::msg::Pose & current_pose,
const float approaching_vel, const float approach_margin, const PathWithLaneId & resampled_path,
PathWithLaneId & output_path);
const float approaching_vel, const float approach_margin, PathWithLaneId & output_path);

void applyMaxJerkLimit(
const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc,
Expand Down
70 changes: 33 additions & 37 deletions planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -101,8 +101,7 @@ bool RunOutModule::modifyPathVelocity(
dynamic_obstacle, current_pose, current_vel, current_acc, trim_smoothed_path, *path);
} else {
// just insert zero velocity for stopping
insertStoppingVelocity(
dynamic_obstacle, current_pose, current_vel, current_acc, trim_smoothed_path, *path);
insertStoppingVelocity(dynamic_obstacle, current_pose, current_vel, current_acc, *path);
}

// apply max jerk limit if the ego can't stop with specified max jerk and acc
Expand Down Expand Up @@ -560,17 +559,20 @@ boost::optional<geometry_msgs::msg::Pose> RunOutModule::calcStopPoint(
// insert the stop point without considering the distance from the obstacle
// smoother will calculate appropriate jerk for deceleration
if (!planner_param_.run_out.specify_decel_jerk) {
// calculate index of stop point
// calculate the stop point for base link
const float base_to_collision_point =
planner_param_.run_out.stop_margin + planner_param_.vehicle_param.base_to_front;
const size_t stop_index = run_out_utils::calcIndexByLengthReverse(
path.points, dynamic_obstacle->nearest_collision_point, base_to_collision_point);
const auto & stop_point = path.points.at(stop_index).point.pose;
const auto stop_point = motion_utils::calcLongitudinalOffsetPose(
path.points, dynamic_obstacle->nearest_collision_point, -base_to_collision_point, false);
if (!stop_point) {
RCLCPP_WARN_STREAM(logger_, "failed to calculate stop point.");
return {};
}

// debug
debug_ptr_->setAccelReason(RunOutDebug::AccelReason::STOP);
debug_ptr_->pushStopPose(tier4_autoware_utils::calcOffsetPose(
stop_point, planner_param_.vehicle_param.base_to_front, 0, 0));
*stop_point, planner_param_.vehicle_param.base_to_front, 0, 0));

return stop_point;
}
Expand All @@ -591,16 +593,7 @@ boost::optional<geometry_msgs::msg::Pose> RunOutModule::calcStopPoint(
stop_dist = boost::make_optional<double>(dist_to_collision);
}

// debug
{
const float base_to_obstacle =
planner_param_.vehicle_param.base_to_front + planner_param_.run_out.stop_margin;
const auto vehicle_stop_idx = run_out_utils::calcIndexByLength(
path.points, current_pose, stop_dist.get() + base_to_obstacle);
const auto & p = path.points.at(vehicle_stop_idx).point.pose.position;
debug_ptr_->pushDebugPoints(p, PointType::Yellow);
debug_ptr_->setDebugValues(DebugValues::TYPE::STOP_DISTANCE, *stop_dist);
}
debug_ptr_->setDebugValues(DebugValues::TYPE::STOP_DISTANCE, *stop_dist);

// vehicle have to decelerate if there is not enough distance with deceleration_jerk
const bool deceleration_needed =
Expand All @@ -615,17 +608,20 @@ boost::optional<geometry_msgs::msg::Pose> RunOutModule::calcStopPoint(
return {};
}

// calculate index of stop point
// calculate the stop point for base link
const float base_to_collision_point =
planner_param_.run_out.stop_margin + planner_param_.vehicle_param.base_to_front;
const size_t stop_index = run_out_utils::calcIndexByLengthReverse(
path.points, dynamic_obstacle->nearest_collision_point, base_to_collision_point);
const auto & stop_point = path.points.at(stop_index).point.pose;
const auto stop_point = motion_utils::calcLongitudinalOffsetPose(
path.points, dynamic_obstacle->nearest_collision_point, -base_to_collision_point, false);
if (!stop_point) {
RCLCPP_WARN_STREAM(logger_, "failed to calculate stop point.");
return {};
}

// debug
debug_ptr_->setAccelReason(RunOutDebug::AccelReason::STOP);
debug_ptr_->pushStopPose(tier4_autoware_utils::calcOffsetPose(
stop_point, planner_param_.vehicle_param.base_to_front, 0, 0));
*stop_point, planner_param_.vehicle_param.base_to_front, 0, 0));

return stop_point;
}
Expand Down Expand Up @@ -691,8 +687,7 @@ void RunOutModule::insertVelocity(
state_ = State::STOP;
}

insertStoppingVelocity(
dynamic_obstacle, current_pose, current_vel, current_acc, smoothed_path, output_path);
insertStoppingVelocity(dynamic_obstacle, current_pose, current_vel, current_acc, output_path);
break;
}

Expand All @@ -703,16 +698,15 @@ void RunOutModule::insertVelocity(
elapsed_time > planner_param_.approaching.stop_time_thresh ? State::APPROACH : State::STOP;
RCLCPP_DEBUG_STREAM(logger_, "elapsed time: " << elapsed_time);

insertStoppingVelocity(
dynamic_obstacle, current_pose, current_vel, current_acc, smoothed_path, output_path);
insertStoppingVelocity(dynamic_obstacle, current_pose, current_vel, current_acc, output_path);
break;
}

case State::APPROACH: {
RCLCPP_DEBUG_STREAM(logger_, "APPROACH state");
insertApproachingVelocity(
*dynamic_obstacle, current_pose, planner_param_.approaching.limit_vel_kmph / 3.6,
planner_param_.approaching.margin, smoothed_path, output_path);
planner_param_.approaching.margin, output_path);
debug_ptr_->setAccelReason(RunOutDebug::AccelReason::STOP);
break;
}
Expand All @@ -727,17 +721,16 @@ void RunOutModule::insertVelocity(
void RunOutModule::insertStoppingVelocity(
const boost::optional<DynamicObstacle> & dynamic_obstacle,
const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc,
const PathWithLaneId & smoothed_path, PathWithLaneId & output_path)
PathWithLaneId & output_path)
{
const auto stop_point =
calcStopPoint(dynamic_obstacle, smoothed_path, current_pose, current_vel, current_acc);
calcStopPoint(dynamic_obstacle, output_path, current_pose, current_vel, current_acc);
insertStopPoint(stop_point, output_path);
}

void RunOutModule::insertApproachingVelocity(
const DynamicObstacle & dynamic_obstacle, const geometry_msgs::msg::Pose & current_pose,
const float approaching_vel, const float approach_margin, const PathWithLaneId & resampled_path,
PathWithLaneId & output_path)
const float approaching_vel, const float approach_margin, PathWithLaneId & output_path)
{
// insert slow down velocity from nearest segment point
const auto nearest_seg_idx =
Expand All @@ -752,23 +745,26 @@ void RunOutModule::insertApproachingVelocity(
// calculate stop point to insert 0 velocity
const float base_to_collision_point =
approach_margin + planner_param_.vehicle_param.base_to_front;
const auto stop_idx = run_out_utils::calcIndexByLengthReverse(
resampled_path.points, dynamic_obstacle.nearest_collision_point, base_to_collision_point);
const auto & stop_point = resampled_path.points.at(stop_idx).point.pose;
const auto stop_point = motion_utils::calcLongitudinalOffsetPose(
output_path.points, dynamic_obstacle.nearest_collision_point, -base_to_collision_point, false);
if (!stop_point) {
RCLCPP_WARN_STREAM(logger_, "failed to calculate stop point.");
return;
}

// debug
debug_ptr_->pushStopPose(tier4_autoware_utils::calcOffsetPose(
stop_point, planner_param_.vehicle_param.base_to_front, 0, 0));
*stop_point, planner_param_.vehicle_param.base_to_front, 0, 0));

const auto nearest_seg_idx_stop =
motion_utils::findNearestSegmentIndex(output_path.points, stop_point.position);
motion_utils::findNearestSegmentIndex(output_path.points, stop_point->position);
auto insert_idx_stop = nearest_seg_idx_stop + 1;

// to PathPointWithLaneId
// use lane id of point behind inserted point
autoware_auto_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id;
stop_point_with_lane_id = output_path.points.at(nearest_seg_idx_stop);
stop_point_with_lane_id.point.pose = stop_point;
stop_point_with_lane_id.point.pose = *stop_point;

planning_utils::insertVelocity(output_path, stop_point_with_lane_id, 0.0, insert_idx_stop);
}
Expand Down