Skip to content

Commit

Permalink
fix(dynamic_avoidance): minor changes with bug fix (#4567)
Browse files Browse the repository at this point in the history 
* feat(dynamic_avoidance): use hatched road markings

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* add some ros parameters

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* update config

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* remove comment

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* feat(dynamic_avoidance): add minor changes

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
  • Loading branch information
@takayuki5168
takayuki5168 authored Aug 8, 2023
1 parent a195b05 commit 9ac8faf
Show file tree
Hide file tree
Showing 2 changed files with 21 additions and 47 deletions.
7 changes: 4 additions & 3 deletions ...include/behavior_path_planner/scene_module/dynamic_avoidance/dynamic_avoidance_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -120,8 +120,10 @@ class DynamicAvoidanceModule : public SceneModuleInterface
std::optional<rclcpp::Time> latest_time_inside_ego_path{std::nullopt};
std::vector<autoware_auto_perception_msgs::msg::PredictedPath> predicted_paths{};

MinMaxValue lon_offset_to_avoid;
MinMaxValue lat_offset_to_avoid;
// NOTE: Previous values of the following are used for low-pass filtering.
// Therefore, they has to be initialized as nullopt.
std::optional<MinMaxValue> lon_offset_to_avoid{std::nullopt};
std::optional<MinMaxValue> lat_offset_to_avoid{std::nullopt};
bool is_collision_left;
bool should_be_avoided{false};

Expand Down Expand Up @@ -306,7 +308,6 @@ class DynamicAvoidanceModule : public SceneModuleInterface

bool isLabelTargetObstacle(const uint8_t label) const;
void updateTargetObjects();
std::optional<std::vector<PathPointWithLaneId>> calcPathForObjectPolygon() const;
bool willObjectCutIn(
const std::vector<PathPointWithLaneId> & ego_path, const PredictedPath & predicted_path,
const double obj_tangent_vel, const LatLonOffset & lat_lon_offset) const;
Expand Down
61 changes: 17 additions & 44 deletions ...ing/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ void appendExtractedPolygonMarker(
auto marker = tier4_autoware_utils::createDefaultMarker(
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), "extracted_polygons", marker_array.markers.size(),
visualization_msgs::msg::Marker::LINE_STRIP,
tier4_autoware_utils::createMarkerScale(0.05, 0.0, 0.0),
tier4_autoware_utils::createMarkerScale(0.1, 0.0, 0.0),
tier4_autoware_utils::createMarkerColor(1.0, 0.5, 0.6, 0.8));

// NOTE: obj_poly.outer() has already duplicated points to close the polygon.
Expand Down Expand Up @@ -349,11 +349,7 @@ void DynamicAvoidanceModule::updateTargetObjects()
const auto prev_module_path = getPreviousModuleOutput().path;
const auto & predicted_objects = planner_data_->dynamic_object->objects;

const auto path_points_for_object_polygon = calcPathForObjectPolygon();
if (!path_points_for_object_polygon) {
return;
}

const auto path_points_for_object_polygon = getPreviousModuleOutput().reference_path->points;
const auto prev_objects = target_objects_manager_.getValidObjects();

// 1. Rough filtering of target objects
Expand Down Expand Up @@ -509,40 +505,16 @@ void DynamicAvoidanceModule::updateTargetObjects()
// 2.g. calculate longitudinal and lateral offset to avoid
const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape);
const auto lon_offset_to_avoid = calcMinMaxLongitudinalOffsetToAvoid(
*path_points_for_object_polygon, object.pose, obj_points, object.vel, time_to_collision);
path_points_for_object_polygon, object.pose, obj_points, object.vel, time_to_collision);
const auto lat_offset_to_avoid = calcMinMaxLateralOffsetToAvoid(
*path_points_for_object_polygon, obj_points, is_collision_left, object.lat_vel, prev_object);
path_points_for_object_polygon, obj_points, is_collision_left, object.lat_vel, prev_object);

const bool should_be_avoided = true;
target_objects_manager_.updateObject(
obj_uuid, lon_offset_to_avoid, lat_offset_to_avoid, is_collision_left, should_be_avoided);
}
}

std::optional<std::vector<PathPointWithLaneId>> DynamicAvoidanceModule::calcPathForObjectPolygon()
const
{
const auto & ego_pose = getEgoPose();
const auto & rh = planner_data_->route_handler;

// get path with backward margin
lanelet::ConstLanelet current_lane;
if (!rh->getClosestLaneletWithinRoute(ego_pose, &current_lane)) {
RCLCPP_ERROR(
rclcpp::get_logger("behavior_path_planner").get_child("dynamic_avoidance"),
"failed to find closest lanelet within route!!!");
return std::nullopt;
}

constexpr double forward_length = 100.0;
const double backward_length = 50.0;
const auto current_lanes =
rh->getLaneletSequence(current_lane, ego_pose, backward_length, forward_length);
const auto path = utils::getCenterLinePath(
*rh, current_lanes, ego_pose, backward_length, forward_length, planner_data_->parameters);
return path.points;
}

[[maybe_unused]] std::optional<std::pair<size_t, size_t>>
DynamicAvoidanceModule::calcCollisionSection(
const std::vector<PathPointWithLaneId> & ego_path, const PredictedPath & obj_path) const
Expand Down Expand Up @@ -849,10 +821,10 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid(

// filter min_bound_lat_offset
const auto prev_min_lat_avoid_to_offset = [&]() -> std::optional<double> {
if (!prev_object) {
if (!prev_object || !prev_object->lat_offset_to_avoid) {
return std::nullopt;
}
return prev_object->lat_offset_to_avoid.min_value;
return prev_object->lat_offset_to_avoid->min_value;
}();
const double filtered_min_bound_lat_offset =
prev_min_lat_avoid_to_offset ? signal_processing::lowpassFilter(
Expand All @@ -867,22 +839,23 @@ MinMaxValue DynamicAvoidanceModule::calcMinMaxLateralOffsetToAvoid(
std::optional<tier4_autoware_utils::Polygon2d> DynamicAvoidanceModule::calcDynamicObstaclePolygon(
const DynamicAvoidanceObject & object) const
{
auto path_points_for_object_polygon = calcPathForObjectPolygon();
if (!path_points_for_object_polygon) {
if (!object.lon_offset_to_avoid || !object.lat_offset_to_avoid) {
return std::nullopt;
}

auto path_points_for_object_polygon = getPreviousModuleOutput().reference_path->points;

const size_t obj_seg_idx =
motion_utils::findNearestSegmentIndex(*path_points_for_object_polygon, object.pose.position);
motion_utils::findNearestSegmentIndex(path_points_for_object_polygon, object.pose.position);
const auto obj_points = tier4_autoware_utils::toPolygon2d(object.pose, object.shape);

const auto lon_bound_start_idx_opt = motion_utils::insertTargetPoint(
obj_seg_idx, object.lon_offset_to_avoid.min_value, *path_points_for_object_polygon);
obj_seg_idx, object.lon_offset_to_avoid->min_value, path_points_for_object_polygon);
const size_t updated_obj_seg_idx =
(lon_bound_start_idx_opt && lon_bound_start_idx_opt.value() <= obj_seg_idx) ? obj_seg_idx + 1
: obj_seg_idx;
const auto lon_bound_end_idx_opt = motion_utils::insertTargetPoint(
updated_obj_seg_idx, object.lon_offset_to_avoid.max_value, *path_points_for_object_polygon);
updated_obj_seg_idx, object.lon_offset_to_avoid->max_value, path_points_for_object_polygon);

if (!lon_bound_start_idx_opt && !lon_bound_end_idx_opt) {
// NOTE: The obstacle is longitudinally out of the ego's trajectory.
Expand All @@ -892,19 +865,19 @@ std::optional<tier4_autoware_utils::Polygon2d> DynamicAvoidanceModule::calcDynam
lon_bound_start_idx_opt ? lon_bound_start_idx_opt.value() : static_cast<size_t>(0);
const size_t lon_bound_end_idx =
lon_bound_end_idx_opt ? lon_bound_end_idx_opt.value()
: static_cast<size_t>(path_points_for_object_polygon->size() - 1);
: static_cast<size_t>(path_points_for_object_polygon.size() - 1);

// create inner/outer bound points
std::vector<geometry_msgs::msg::Point> obj_inner_bound_points;
std::vector<geometry_msgs::msg::Point> obj_outer_bound_points;
for (size_t i = lon_bound_start_idx; i <= lon_bound_end_idx; ++i) {
obj_inner_bound_points.push_back(tier4_autoware_utils::calcOffsetPose(
path_points_for_object_polygon->at(i).point.pose, 0.0,
object.lat_offset_to_avoid.min_value, 0.0)
path_points_for_object_polygon.at(i).point.pose, 0.0,
object.lat_offset_to_avoid->min_value, 0.0)
.position);
obj_outer_bound_points.push_back(tier4_autoware_utils::calcOffsetPose(
path_points_for_object_polygon->at(i).point.pose, 0.0,
object.lat_offset_to_avoid.max_value, 0.0)
path_points_for_object_polygon.at(i).point.pose, 0.0,
object.lat_offset_to_avoid->max_value, 0.0)
.position);
}

Expand Down

0 comments on commit 9ac8faf

Please sign in to comment.