Skip to content

Commit

Permalink
feat(behavior_velocity_planner): changeable maximum stop distance (ti…
Browse files Browse the repository at this point in the history 
…er4#1380)

* fix(behavior_velocity_planner): changeable max stop distance

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

* fix(behavior_velocity_planner): use alias

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

* fix(behavior_velocity_planner): update default param

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

* fix(behavior_velocity_plannerl): rename param

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

* fix(behaivor_velocity_planner): fix current stop position

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
  • Loading branch information
@satoshi-ota @boyali
satoshi-ota authored and boyali committed Oct 19, 2022
1 parent 665b96b commit d73269c
Show file tree
Hide file tree
Showing 4 changed files with 61 additions and 53 deletions.
1 change: 1 addition & 0 deletions planning/behavior_velocity_planner/config/detection_area.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,3 +6,4 @@
dead_line_margin: 5.0
use_pass_judge_line: false
state_clear_time: 2.0
hold_stop_margin_distance: 0.0
19 changes: 9 additions & 10 deletions planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,9 @@ namespace behavior_velocity_planner
using PathIndexWithPose = std::pair<size_t, geometry_msgs::msg::Pose>; // front index, pose
using PathIndexWithPoint2d = std::pair<size_t, Point2d>; // front index, point2d
using PathIndexWithOffset = std::pair<size_t, double>; // front index, offset
using autoware_auto_planning_msgs::msg::PathWithLaneId;
using tier4_planning_msgs::msg::StopFactor;
using tier4_planning_msgs::msg::StopReason;

class DetectionAreaModule : public SceneModuleInterface
{
Expand All @@ -58,6 +61,7 @@ class DetectionAreaModule : public SceneModuleInterface
double dead_line_margin;
bool use_pass_judge_line;
double state_clear_time;
double hold_stop_margin_distance;
};

public:
Expand All @@ -66,9 +70,7 @@ class DetectionAreaModule : public SceneModuleInterface
const PlannerParam & planner_param, const rclcpp::Logger logger,
const rclcpp::Clock::SharedPtr clock);

bool modifyPathVelocity(
autoware_auto_planning_msgs::msg::PathWithLaneId * path,
tier4_planning_msgs::msg::StopReason * stop_reason) override;
bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override;

visualization_msgs::msg::MarkerArray createDebugMarkerArray() override;
visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override;
Expand All @@ -81,19 +83,16 @@ class DetectionAreaModule : public SceneModuleInterface
bool canClearStopState() const;

bool isOverLine(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const;
const PathWithLaneId & path, const geometry_msgs::msg::Pose & self_pose,
const geometry_msgs::msg::Pose & line_pose) const;

bool hasEnoughBrakingDistance(
const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const;

autoware_auto_planning_msgs::msg::PathWithLaneId insertStopPoint(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const PathIndexWithPose & stop_point) const;
void insertStopPoint(const geometry_msgs::msg::Pose & stop_pose, PathWithLaneId & path) const;

boost::optional<PathIndexWithPose> createTargetPoint(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line,
const double margin) const;
const PathWithLaneId & path, const LineString2d & stop_line, const double margin) const;

// Key Feature
const lanelet::autoware::DetectionArea & detection_area_reg_elem_;
Expand Down
2 changes: 2 additions & 0 deletions planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,6 +37,8 @@ DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node)
planner_param_.dead_line_margin = node.declare_parameter(ns + ".dead_line_margin", 5.0);
planner_param_.use_pass_judge_line = node.declare_parameter(ns + ".use_pass_judge_line", false);
planner_param_.state_clear_time = node.declare_parameter(ns + ".state_clear_time", 2.0);
planner_param_.hold_stop_margin_distance =
node.declare_parameter(ns + ".hold_stop_margin_distance", 0.0);
}

void DetectionAreaModuleManager::launchNewModules(
Expand Down
92 changes: 49 additions & 43 deletions planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,10 @@
namespace behavior_velocity_planner
{
namespace bg = boost::geometry;
using motion_utils::calcLongitudinalOffsetPose;
using motion_utils::calcSignedArcLength;
using motion_utils::findNearestSegmentIndex;
using motion_utils::insertTargetPoint;

namespace
{
Expand Down Expand Up @@ -74,7 +78,7 @@ boost::optional<Point2d> getNearestCollisionPoint(
}

boost::optional<PathIndexWithPoint2d> findCollisionSegment(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line)
const PathWithLaneId & path, const LineString2d & stop_line)
{
for (size_t i = 0; i < path.points.size() - 1; ++i) {
const auto & p1 = path.points.at(i).point.pose.position; // Point before collision point
Expand All @@ -92,8 +96,7 @@ boost::optional<PathIndexWithPoint2d> findCollisionSegment(
}

boost::optional<PathIndexWithOffset> findForwardOffsetSegment(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t base_idx,
const double offset_length)
const PathWithLaneId & path, const size_t base_idx, const double offset_length)
{
double sum_length = 0.0;
for (size_t i = base_idx; i < path.points.size() - 1; ++i) {
Expand All @@ -115,8 +118,7 @@ boost::optional<PathIndexWithOffset> findForwardOffsetSegment(
}

boost::optional<PathIndexWithOffset> findBackwardOffsetSegment(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t base_idx,
const double offset_length)
const PathWithLaneId & path, const size_t base_idx, const double offset_length)
{
double sum_length = 0.0;
const auto start = static_cast<std::int32_t>(base_idx) - 1;
Expand All @@ -138,8 +140,8 @@ boost::optional<PathIndexWithOffset> findBackwardOffsetSegment(
}

boost::optional<PathIndexWithOffset> findOffsetSegment(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const PathIndexWithPoint2d & collision_segment, const double offset_length)
const PathWithLaneId & path, const PathIndexWithPoint2d & collision_segment,
const double offset_length)
{
const size_t & collision_idx = collision_segment.first;
const Point2d & collision_point = collision_segment.second;
Expand All @@ -156,8 +158,7 @@ boost::optional<PathIndexWithOffset> findOffsetSegment(
}

geometry_msgs::msg::Pose calcTargetPose(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const PathIndexWithOffset & offset_segment)
const PathWithLaneId & path, const PathIndexWithOffset & offset_segment)
{
const size_t offset_idx = offset_segment.first;
const double remain_offset_length = offset_segment.second;
Expand Down Expand Up @@ -208,18 +209,15 @@ LineString2d DetectionAreaModule::getStopLineGeometry2d() const
stop_line[0], stop_line[1], planner_data_->stop_line_extend_length);
}

bool DetectionAreaModule::modifyPathVelocity(
autoware_auto_planning_msgs::msg::PathWithLaneId * path,
tier4_planning_msgs::msg::StopReason * stop_reason)
bool DetectionAreaModule::modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason)
{
// Store original path
const auto original_path = *path;

// Reset data
debug_data_ = DebugData();
debug_data_.base_link2front = planner_data_->vehicle_info_.max_longitudinal_offset_m;
*stop_reason =
planning_utils::initializeStopReason(tier4_planning_msgs::msg::StopReason::DETECTION_AREA);
*stop_reason = planning_utils::initializeStopReason(StopReason::DETECTION_AREA);

// Find obstacles in detection area
const auto obstacle_points = getObstaclePoints();
Expand All @@ -240,10 +238,24 @@ bool DetectionAreaModule::modifyPathVelocity(
return true;
}

const auto & stop_pose = stop_point->second;
const auto is_stopped = planner_data_->isVehicleStopped(0.0);

auto stop_pose = stop_point->second;
const auto stop_dist = calcSignedArcLength(path->points, self_pose.position, stop_pose.position);

setDistance(
motion_utils::calcSignedArcLength(path->points, self_pose.position, stop_pose.position));
// Don't re-approach when the ego stops closer to the stop point than hold_stop_margin_distance
if (is_stopped && stop_dist < planner_param_.hold_stop_margin_distance) {
const auto ego_pos_on_path =
calcLongitudinalOffsetPose(original_path.points, self_pose.position, 0.0);

if (!ego_pos_on_path) {
return false;
}

stop_pose = ego_pos_on_path.get();
}

setDistance(stop_dist);

// Check state
setSafe(canClearStopState());
Expand Down Expand Up @@ -272,14 +284,14 @@ bool DetectionAreaModule::modifyPathVelocity(
}

// Ignore objects detected after stop_line if not in STOP state
if (state_ != State::STOP && isOverLine(original_path, self_pose, stop_pose)) {
if (state_ != State::STOP && isOverLine(original_path, self_pose, stop_point->second)) {
setSafe(true);
return true;
}

// Ignore objects if braking distance is not enough
if (planner_param_.use_pass_judge_line) {
if (state_ != State::STOP && !hasEnoughBrakingDistance(self_pose, stop_pose)) {
if (state_ != State::STOP && !hasEnoughBrakingDistance(self_pose, stop_point->second)) {
RCLCPP_WARN_THROTTLE(
logger_, *clock_, std::chrono::milliseconds(1000).count(),
"[detection_area] vehicle is over stop border");
Expand All @@ -290,14 +302,14 @@ bool DetectionAreaModule::modifyPathVelocity(

// Insert stop point
state_ = State::STOP;
*path = insertStopPoint(original_path, *stop_point);
insertStopPoint(stop_pose, *path);

// For virtual wall
debug_data_.stop_poses.push_back(stop_pose);
debug_data_.stop_poses.push_back(stop_point->second);

// Create StopReason
{
tier4_planning_msgs::msg::StopFactor stop_factor;
StopFactor stop_factor{};
stop_factor.stop_pose = stop_point->second;
stop_factor.stop_factor_points = obstacle_points;
planning_utils::appendStopReason(stop_factor, stop_reason);
Expand All @@ -310,7 +322,7 @@ bool DetectionAreaModule::modifyPathVelocity(
if (
!first_stop_path_point_index_ ||
static_cast<int>(insert_idx) < first_stop_path_point_index_) {
debug_data_.first_stop_pose = stop_pose;
debug_data_.first_stop_pose = stop_point->second;
first_stop_path_point_index_ = static_cast<int>(insert_idx);
}
}
Expand Down Expand Up @@ -360,10 +372,10 @@ bool DetectionAreaModule::canClearStopState() const
}

bool DetectionAreaModule::isOverLine(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const
const PathWithLaneId & path, const geometry_msgs::msg::Pose & self_pose,
const geometry_msgs::msg::Pose & line_pose) const
{
return motion_utils::calcSignedArcLength(path.points, self_pose.position, line_pose.position) < 0;
return calcSignedArcLength(path.points, self_pose.position, line_pose.position) < 0;
}

bool DetectionAreaModule::hasEnoughBrakingDistance(
Expand All @@ -379,29 +391,23 @@ bool DetectionAreaModule::hasEnoughBrakingDistance(
return calcSignedDistance(self_pose, line_pose.position) > pass_judge_line_distance;
}

autoware_auto_planning_msgs::msg::PathWithLaneId DetectionAreaModule::insertStopPoint(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const PathIndexWithPose & stop_point) const
void DetectionAreaModule::insertStopPoint(
const geometry_msgs::msg::Pose & stop_pose, PathWithLaneId & path) const
{
auto output_path = path;
const size_t base_idx = findNearestSegmentIndex(path.points, stop_pose.position);
const auto insert_idx = insertTargetPoint(base_idx, stop_pose.position, path.points);

size_t insert_idx = static_cast<size_t>(stop_point.first + 1);
const auto stop_pose = stop_point.second;

// To PathPointWithLaneId
autoware_auto_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id;
stop_point_with_lane_id = output_path.points.at(insert_idx);
stop_point_with_lane_id.point.pose = stop_pose;
stop_point_with_lane_id.point.longitudinal_velocity_mps = 0.0;
if (!insert_idx) {
return;
}

// Insert stop point or replace with zero velocity
planning_utils::insertVelocity(output_path, stop_point_with_lane_id, 0.0, insert_idx);
return output_path;
for (size_t i = insert_idx.get(); i < path.points.size(); ++i) {
path.points.at(i).point.longitudinal_velocity_mps = 0.0;
}
}

boost::optional<PathIndexWithPose> DetectionAreaModule::createTargetPoint(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const LineString2d & stop_line,
const double margin) const
const PathWithLaneId & path, const LineString2d & stop_line, const double margin) const
{
// Find collision segment
const auto collision_segment = findCollisionSegment(path, stop_line);
Expand Down

0 comments on commit d73269c

Please sign in to comment.