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

fix(behavior_velocity): deadlock condition #240

Merged
merged 3 commits into from
Jan 25, 2022
Merged

fix(behavior_velocity): deadlock condition #240

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
b950eac
fix(behavior_velocity): fix dead lock condition
taikitanaka3 Jan 25, 2022
76c7ce9
chore(behavior_velocity): unite valiable name to stop line
taikitanaka3 Jan 25, 2022
abbf53a
fix(behavior_velocity): fix dead lock condition of merge from private
taikitanaka3 Jan 25, 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
18 changes: 11 additions & 7 deletions planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
// limitations under the License.

#include <scene_module/crosswalk/scene_walkway.hpp>
#include <tier4_autoware_utils/trajectory/trajectory.hpp>
#include <utilization/util.hpp>

#include <cmath>
Expand Down Expand Up @@ -80,18 +81,21 @@ bool WalkwayModule::modifyPathVelocity(
stop_factor.stop_factor_points.emplace_back(debug_data_.nearest_collision_point);
planning_utils::appendStopReason(stop_factor, stop_reason);

// update state
const Point self_pose = {
planner_data_->current_pose.pose.position.x, planner_data_->current_pose.pose.position.y};
const Point stop_pose = {
debug_data_.first_stop_pose.position.x, debug_data_.first_stop_pose.position.y};
const double distance = bg::distance(stop_pose, self_pose);
// use arc length to identify if ego vehicle is in front of walkway stop or not.
const double signed_arc_dist_to_stop_point = tier4_autoware_utils::calcSignedArcLength(
path->points, planner_data_->current_pose.pose.position,
debug_data_.first_stop_pose.position);
const double distance_threshold = 1.0;
debug_data_.stop_judge_range = distance_threshold;
if (
distance < distance_threshold &&
signed_arc_dist_to_stop_point < distance_threshold &&
planner_data_->isVehicleStopped(planner_param_.stop_duration_sec)) {
// If ego vehicle is after walkway stop and stopped then move to stop state
state_ = State::STOP;
if (signed_arc_dist_to_stop_point < -distance_threshold) {
RCLCPP_ERROR(
logger_, "Failed to stop near walkway but ego stopped. Change state to STOPPED");
}
}
return true;
} else if (state_ == State::STOP) {
Expand Down
10 changes: 7 additions & 3 deletions ...behavior_velocity_planner/src/scene_module/intersection/scene_merge_from_private_road.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -120,13 +120,17 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(
planning_utils::appendStopReason(stop_factor, stop_reason);
}

const double distance =
planning_utils::calcDist2d(current_pose.pose, path->points.at(stop_line_idx).point.pose);
const double signed_arc_dist_to_stop_point = tier4_autoware_utils::calcSignedArcLength(
path->points, current_pose.pose.position, path->points.at(stop_line_idx).point.pose.position);

constexpr double distance_threshold = 2.0;
if (
distance < distance_threshold &&
signed_arc_dist_to_stop_point < distance_threshold &&
planner_data_->isVehicleStopped(planner_param_.stop_duration_sec)) {
state_machine_.setState(State::GO);
if (signed_arc_dist_to_stop_point < -distance_threshold) {
RCLCPP_ERROR(logger_, "Failed to stop near stop line but ego stopped. Change state to GO");
}
}

return true;
Expand Down
77 changes: 8 additions & 69 deletions planning/behavior_velocity_planner/src/scene_module/stop_line/scene.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
// limitations under the License.

#include <scene_module/stop_line/scene.hpp>
#include <tier4_autoware_utils/trajectory/trajectory.hpp>
#include <utilization/util.hpp>

#include <algorithm>
Expand Down Expand Up @@ -87,72 +88,6 @@ boost::optional<StopLineModule::SegmentIndexWithOffset> findBackwardOffsetSegmen
return {};
}

StopLineModule::SegmentIndexWithOffset findNearestSegment(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const geometry_msgs::msg::Point & point)
{
std::vector<double> distances;
distances.reserve(path.points.size());

std::transform(
path.points.cbegin(), path.points.cend(), std::back_inserter(distances),
[&point](const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) {
return planning_utils::calcDist2d(p.point.pose.position, point);
});

const auto min_itr = std::min_element(distances.cbegin(), distances.cend());
const auto min_idx = static_cast<size_t>(std::distance(distances.cbegin(), min_itr));

// Process boundary conditions
if (min_idx == 0 || min_idx == path.points.size() - 1) {
const size_t segment_idx = min_idx == 0 ? min_idx : min_idx - 1;

return StopLineModule::SegmentIndexWithOffset{
segment_idx, planning_utils::calcDist2d(path.points.at(segment_idx), point)};
}

// Process normal conditions(having previous and next points)
const auto & p_prev = path.points.at(min_idx - 1);
const auto d_prev = planning_utils::calcDist2d(p_prev, point);

const auto & p_next = path.points.at(min_idx + 1);
const auto d_next = planning_utils::calcDist2d(p_next, point);

const size_t segment_idx = (d_prev <= d_next) ? min_idx - 1 : min_idx;

return StopLineModule::SegmentIndexWithOffset{
segment_idx, planning_utils::calcDist2d(path.points.at(segment_idx), point)};
}

double calcSignedArcLength(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t & idx_front,
const size_t & idx_back)
{
// If flipped, reverse index and take negative
if (idx_front > idx_back) {
return -calcSignedArcLength(path, idx_back, idx_front);
}

double sum_length = 0.0;
for (size_t i = idx_front; i < idx_back; ++i) {
const auto & p1 = path.points.at(i).point.pose.position;
const auto & p2 = path.points.at(i + 1).point.pose.position;
sum_length += planning_utils::calcDist2d(p1, p2);
}

return sum_length;
}

double calcSignedArcLength(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2)
{
const auto seg_1 = findNearestSegment(path, p1);
const auto seg_2 = findNearestSegment(path, p2);

return -seg_1.offset + calcSignedArcLength(path, seg_1.index, seg_2.index) + seg_2.offset;
}

} // namespace

StopLineModule::StopLineModule(
Expand Down Expand Up @@ -282,19 +217,23 @@ bool StopLineModule::modifyPathVelocity(
const auto stop_pose_with_index = calcStopPose(*path, offset_segment);

// Calc dist to stop pose
const double signed_arc_dist_to_stop_point = calcSignedArcLength(
*path, planner_data_->current_pose.pose.position, stop_pose_with_index->pose.position);
const double signed_arc_dist_to_stop_point = tier4_autoware_utils::calcSignedArcLength(
path->points, planner_data_->current_pose.pose.position, stop_pose_with_index->pose.position);

if (state_ == State::APPROACH) {
// Insert stop pose
*path = insertStopPose(*path, *stop_pose_with_index, stop_reason);

// Move to stopped state if stopped
if (
std::abs(signed_arc_dist_to_stop_point) < planner_param_.stop_check_dist &&
signed_arc_dist_to_stop_point < planner_param_.stop_check_dist &&
planner_data_->isVehicleStopped(planner_param_.stop_duration_sec)) {
RCLCPP_INFO(logger_, "APPROACH -> STOPPED");
state_ = State::STOPPED;
if (signed_arc_dist_to_stop_point < -planner_param_.stop_check_dist) {
RCLCPP_ERROR(
logger_, "Failed to stop near stop line but ego stopped. Change state to STOPPED");
}
}
} else if (state_ == State::STOPPED) {
// Change state after vehicle departure
Expand Down