Skip to content

Commit

Permalink
fix(behavior_velocity): deadlock condition (#240)
Browse files Browse the repository at this point in the history 
* fix(behavior_velocity): fix dead lock condition

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* chore(behavior_velocity): unite valiable name to stop line

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>

* fix(behavior_velocity): fix dead lock condition of merge from private

Signed-off-by: tanaka3 <ttatcoder@outlook.jp>
  • Loading branch information
@taikitanaka3
taikitanaka3 authored Jan 25, 2022
1 parent 50f6888 commit f49b3c2
Show file tree
Hide file tree
Showing 3 changed files with 26 additions and 79 deletions.
18 changes: 11 additions & 7 deletions planning/behavior_velocity_planner/src/scene_module/crosswalk/scene_walkway.cpp
View file
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
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
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

0 comments on commit f49b3c2

Please sign in to comment.