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.

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

feat(lane_change): not execute LC on crosswalk #872

Merged
merged 2 commits into from
Sep 26, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,11 @@
motorcycle: true
pedestrian: true

# lane change regulations
regulation:
crosswalk: false
intersection: false

# collision check
enable_prepare_segment_collision_check: true
prepare_segment_ignore_object_velocity_thresh: 0.1 # [m/s]
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -186,6 +186,11 @@ The following figure illustrates when the lane is blocked in multiple lane chang

![multiple-lane-changes](../image/lane_change/lane_change-when_cannot_change_lanes.png)

### Lane change regulations

If you want to regulate lane change on crosswalks or intersections, the lane change module finds a lane change path excluding it includes crosswalks or intersections.
To regulate lane change on crosswalks or intersections, change `regulation.crosswalk` or `regulation.intersection` to `true`.

### Aborting lane change

The abort process may result in three different outcome; Cancel, Abort and Stop/Cruise.
Expand Down Expand Up @@ -295,6 +300,13 @@ The following parameters are configurable in `lane_change.param.yaml`.
| `target_object.motorcycle` | [-] | boolean | Include motorcycle objects for safety check | true |
| `target_object.pedestrian` | [-] | boolean | Include pedestrian objects for safety check | true |

### Lane change regulations

| Name | Unit | Type | Description | Default value |
| :------------------------ | ---- | ------- | ------------------------------------- | ------------- |
| `regulation.crosswalk` | [-] | boolean | Regulate lane change on crosswalks | false |
| `regulation.intersection` | [-] | boolean | Regulate lane change on intersections | false |

### Collision checks during lane change

The following parameters are configurable in `behavior_path_planner.param.yaml` and `lane_change.param.yaml`.
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -108,7 +108,7 @@ class NormalLaneChange : public LaneChangeBase
const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes) const;

double calcPrepareDuration(
std::vector<double> calcPrepareDuration(
const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes) const;

Expand All @@ -129,6 +129,12 @@ class NormalLaneChange : public LaneChangeBase
const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes, const Direction direction = Direction::NONE) const;

bool hasEnoughLengthToCrosswalk(
const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const;

bool hasEnoughLengthToIntersection(
const LaneChangePath & path, const lanelet::ConstLanelets & current_lanes) const;

bool getLaneChangePaths(
const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
Direction direction, LaneChangePaths * candidate_paths,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -66,15 +66,12 @@ struct LaneChangeParameters
bool check_objects_on_other_lanes{true};
bool use_all_predicted_path{false};

// true by default
bool check_car{true}; // check object car
bool check_truck{true}; // check object truck
bool check_bus{true}; // check object bus
bool check_trailer{true}; // check object trailer
bool check_unknown{true}; // check object unknown
bool check_bicycle{true}; // check object bicycle
bool check_motorcycle{true}; // check object motorbike
bool check_pedestrian{true}; // check object pedestrian
// regulatory elements
bool regulate_on_crosswalk{false};
bool regulate_on_intersection{false};

// true by default for all objects
utils::path_safety_checker::ObjectTypesToCheck object_types_to_check;

// safety check
utils::path_safety_checker::RSSparams rss_params;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -142,8 +142,6 @@ lanelet::ConstLanelets getBackwardLanelets(
const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
const Pose & current_pose, const double backward_length);

bool isTargetObjectType(const PredictedObject & object, const LaneChangeParameters & parameters);

double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer = 0.0);

double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer);
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -187,6 +187,20 @@ TargetObjectsOnLane createTargetObjectsOnLane(
const PredictedObjects & filtered_objects,
const std::shared_ptr<ObjectsFilteringParams> & params);

/**
* @brief Determines whether the predicted object type matches any of the target object types
* specified by the user.
*
* @param object The predicted object whose type is to be checked.
* @param target_object_types A structure containing boolean flags for each object type that the
* user is interested in checking.
*
* @return Returns true if the predicted object's highest probability label matches any of the
* specified target object types.
*/
bool isTargetObjectType(
const PredictedObject & object, const ObjectTypesToCheck & target_object_types);

} // namespace behavior_path_planner::utils::path_safety_checker

#endif // BEHAVIOR_PATH_PLANNER__UTILS__PATH_SAFETY_CHECKER__OBJECTS_FILTERING_HPP_
Original file line number Diff line number Diff line change
Expand Up @@ -75,6 +75,11 @@ LaneChangeModuleManager::LaneChangeModuleManager(
p.use_all_predicted_path =
getOrDeclareParameter<bool>(*node, parameter("use_all_predicted_path"));

// lane change regulations
p.regulate_on_crosswalk = getOrDeclareParameter<bool>(*node, parameter("regulation.crosswalk"));
p.regulate_on_intersection =
getOrDeclareParameter<bool>(*node, parameter("regulation.intersection"));

p.rss_params.longitudinal_distance_min_threshold = getOrDeclareParameter<double>(
*node, parameter("safety_check.longitudinal_distance_min_threshold"));
p.rss_params.longitudinal_velocity_delta_time = getOrDeclareParameter<double>(
Expand Down Expand Up @@ -108,14 +113,16 @@ LaneChangeModuleManager::LaneChangeModuleManager(
// target object
{
std::string ns = "lane_change.target_object.";
p.check_car = getOrDeclareParameter<bool>(*node, ns + "car");
p.check_truck = getOrDeclareParameter<bool>(*node, ns + "truck");
p.check_bus = getOrDeclareParameter<bool>(*node, ns + "bus");
p.check_trailer = getOrDeclareParameter<bool>(*node, ns + "trailer");
p.check_unknown = getOrDeclareParameter<bool>(*node, ns + "unknown");
p.check_bicycle = getOrDeclareParameter<bool>(*node, ns + "bicycle");
p.check_motorcycle = getOrDeclareParameter<bool>(*node, ns + "motorcycle");
p.check_pedestrian = getOrDeclareParameter<bool>(*node, ns + "pedestrian");
p.object_types_to_check.check_car = getOrDeclareParameter<bool>(*node, ns + "car");
p.object_types_to_check.check_truck = getOrDeclareParameter<bool>(*node, ns + "truck");
p.object_types_to_check.check_bus = getOrDeclareParameter<bool>(*node, ns + "bus");
p.object_types_to_check.check_trailer = getOrDeclareParameter<bool>(*node, ns + "trailer");
p.object_types_to_check.check_unknown = getOrDeclareParameter<bool>(*node, ns + "unknown");
p.object_types_to_check.check_bicycle = getOrDeclareParameter<bool>(*node, ns + "bicycle");
p.object_types_to_check.check_motorcycle =
getOrDeclareParameter<bool>(*node, ns + "motorcycle");
p.object_types_to_check.check_pedestrian =
getOrDeclareParameter<bool>(*node, ns + "pedestrian");
}

// lane change cancel
Expand Down
Loading
Loading