Skip to content

Commit

Permalink
feat(behavior_path_planner): consider object velocity direction (auto…
Browse files Browse the repository at this point in the history 
…warefoundation#4652)

* feat(behavior_path_planner): consider object velocity direction

Signed-off-by: Takayuki Murooka <[email protected]>

* update

Signed-off-by: Takayuki Murooka <[email protected]>

* Update planning/behavior_path_planner/src/scene_module/dynamic_avoidance/dynamic_avoidance_module.cpp

---------

Signed-off-by: Takayuki Murooka <[email protected]>
  • Loading branch information
@takayuki5168 @kminoda
takayuki5168 authored and kminoda committed Aug 23, 2023
1 parent b66be62 commit f7cafb1
Show file tree
Hide file tree
Showing 5 changed files with 34 additions and 19 deletions.
17 changes: 12 additions & 5 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 @@ -101,15 +101,20 @@ std::pair<double, double> projectObstacleVelocityToTrajectory(
const std::vector<PathPointWithLaneId> & path_points, const PredictedObject & object)
{
const auto & obj_pose = object.kinematics.initial_pose_with_covariance.pose;
const double obj_vel = object.kinematics.initial_twist_with_covariance.twist.linear.x;
const double obj_vel_norm = std::hypot(
object.kinematics.initial_twist_with_covariance.twist.linear.x,
object.kinematics.initial_twist_with_covariance.twist.linear.y);

const size_t obj_idx = motion_utils::findNearestIndex(path_points, obj_pose.position);

const double obj_yaw = tf2::getYaw(obj_pose.orientation);
const double obj_vel_yaw = std::atan2(
object.kinematics.initial_twist_with_covariance.twist.linear.y,
object.kinematics.initial_twist_with_covariance.twist.linear.x);
const double path_yaw = tf2::getYaw(path_points.at(obj_idx).point.pose.orientation);

return std::make_pair(
obj_vel * std::cos(obj_yaw - path_yaw), obj_vel * std::sin(obj_yaw - path_yaw));
obj_vel_norm * std::cos(obj_vel_yaw - path_yaw),
obj_vel_norm * std::sin(obj_vel_yaw - path_yaw));
}

double calcObstacleMaxLength(const autoware_auto_perception_msgs::msg::Shape & shape)
Expand Down Expand Up @@ -365,7 +370,9 @@ void DynamicAvoidanceModule::updateTargetObjects()
for (const auto & predicted_object : predicted_objects) {
const auto obj_uuid = tier4_autoware_utils::toHexString(predicted_object.object_id);
const auto & obj_pose = predicted_object.kinematics.initial_pose_with_covariance.pose;
const double obj_vel = predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x;
const double obj_vel_norm = std::hypot(
predicted_object.kinematics.initial_twist_with_covariance.twist.linear.x,
predicted_object.kinematics.initial_twist_with_covariance.twist.linear.y);
const auto prev_object = getObstacleFromUuid(prev_objects, obj_uuid);

// 1.a. check label
Expand Down Expand Up @@ -393,7 +400,7 @@ void DynamicAvoidanceModule::updateTargetObjects()
? parameters_->min_overtaking_crossing_object_vel
: parameters_->min_oncoming_crossing_object_vel;
const bool is_crossing_object_to_ignore =
min_crossing_object_vel < std::abs(obj_vel) && is_obstacle_crossing_path;
min_crossing_object_vel < obj_vel_norm && is_obstacle_crossing_path;
if (is_crossing_object_to_ignore) {
RCLCPP_INFO_EXPRESSION(
getLogger(), parameters_->enable_debug_info,
Expand Down
3 changes: 2 additions & 1 deletion planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -336,7 +336,8 @@ std::shared_ptr<LaneChangeDebugMsgArray> LaneChangeInterface::get_debug_msg_arra
debug_msg.is_front = debug_data.is_front;
debug_msg.relative_distance = debug_data.relative_to_ego;
debug_msg.failed_reason = debug_data.failed_reason;
debug_msg.velocity = debug_data.object_twist.linear.x;
debug_msg.velocity =
std::hypot(debug_data.object_twist.linear.x, debug_data.object_twist.linear.y);
debug_msg_array.lane_change_info.push_back(debug_msg);
}
lane_change_debug_msg_array_ = debug_msg_array;
Expand Down
6 changes: 4 additions & 2 deletions planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -677,7 +677,9 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
LaneChangeTargetObjectIndices filtered_obj_indices;
for (size_t i = 0; i < objects.objects.size(); ++i) {
const auto & object = objects.objects.at(i);
const auto & obj_velocity = object.kinematics.initial_twist_with_covariance.twist.linear.x;
const auto & obj_velocity_norm = std::hypot(
object.kinematics.initial_twist_with_covariance.twist.linear.x,
object.kinematics.initial_twist_with_covariance.twist.linear.y);
const auto extended_object =
utils::lane_change::transform(object, common_parameters, *lane_change_parameters_);

Expand All @@ -700,7 +702,7 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
}

// ignore static object that are behind the ego vehicle
if (obj_velocity < 1.0 && max_dist_ego_to_obj < 0.0) {
if (obj_velocity_norm < 1.0 && max_dist_ego_to_obj < 0.0) {
continue;
}

Expand Down
11 changes: 6 additions & 5 deletions planning/behavior_path_planner/src/utils/avoidance/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -693,9 +693,9 @@ void fillObjectMovingTime(
const auto object_type = utils::getHighestProbLabel(object_data.object.classification);
const auto object_parameter = parameters->object_parameters.at(object_type);

const auto & object_vel =
object_data.object.kinematics.initial_twist_with_covariance.twist.linear.x;
const auto is_faster_than_threshold = object_vel > object_parameter.moving_speed_threshold;
const auto & object_twist = object_data.object.kinematics.initial_twist_with_covariance.twist;
const auto object_vel_norm = std::hypot(object_twist.linear.x, object_twist.linear.y);
const auto is_faster_than_threshold = object_vel_norm > object_parameter.moving_speed_threshold;

const auto id = object_data.object.object_id;
const auto same_id_obj = std::find_if(
Expand Down Expand Up @@ -1475,7 +1475,8 @@ ExtendedPredictedObject transform(
extended_object.initial_acceleration = object.kinematics.initial_acceleration_with_covariance;
extended_object.shape = object.shape;

const auto & obj_velocity = extended_object.initial_twist.twist.linear.x;
const auto & obj_velocity_norm = std::hypot(
extended_object.initial_twist.twist.linear.x, extended_object.initial_twist.twist.linear.y);
const auto & time_horizon = parameters->safety_check_time_horizon;
const auto & time_resolution = parameters->safety_check_time_resolution;

Expand All @@ -1490,7 +1491,7 @@ ExtendedPredictedObject transform(
if (obj_pose) {
const auto obj_polygon = tier4_autoware_utils::toPolygon2d(*obj_pose, object.shape);
extended_object.predicted_paths.at(i).path.emplace_back(
t, *obj_pose, obj_velocity, obj_polygon);
t, *obj_pose, obj_velocity_norm, obj_polygon);
}
}
}
Expand Down
16 changes: 10 additions & 6 deletions planning/behavior_path_planner/src/utils/lane_change/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -855,7 +855,9 @@ bool isParkedObject(
using lanelet::geometry::distance2d;
using lanelet::geometry::toArcCoordinates;

if (object.initial_twist.twist.linear.x > static_object_velocity_threshold) {
const double object_vel_norm =
std::hypot(object.initial_twist.twist.linear.x, object.initial_twist.twist.linear.y);
if (object_vel_norm > static_object_velocity_threshold) {
return false;
}

Expand Down Expand Up @@ -1034,7 +1036,9 @@ boost::optional<size_t> getLeadingStaticObjectIdx(

// ignore non-static object
// TODO(shimizu): parametrize threshold
if (obj.initial_twist.twist.linear.x > 1.0) {
const double obj_vel_norm =
std::hypot(obj.initial_twist.twist.linear.x, obj.initial_twist.twist.linear.y);
if (obj_vel_norm > 1.0) {
continue;
}

Expand Down Expand Up @@ -1095,9 +1099,9 @@ ExtendedPredictedObject transform(
const auto & prepare_duration = common_parameters.lane_change_prepare_duration;
const auto & velocity_threshold =
lane_change_parameters.prepare_segment_ignore_object_velocity_thresh;
const auto & obj_vel = extended_object.initial_twist.twist.linear.x;
const auto start_time = check_at_prepare_phase ? 0.0 : prepare_duration;
const double obj_velocity = extended_object.initial_twist.twist.linear.x;
const double obj_vel_norm = std::hypot(
extended_object.initial_twist.twist.linear.x, extended_object.initial_twist.twist.linear.y);

extended_object.predicted_paths.resize(object.kinematics.predicted_paths.size());
for (size_t i = 0; i < object.kinematics.predicted_paths.size(); ++i) {
Expand All @@ -1109,14 +1113,14 @@ ExtendedPredictedObject transform(
// create path
for (double t = start_time; t < end_time + std::numeric_limits<double>::epsilon();
t += time_resolution) {
if (t < prepare_duration && obj_vel < velocity_threshold) {
if (t < prepare_duration && obj_vel_norm < velocity_threshold) {
continue;
}
const auto obj_pose = object_recognition_utils::calcInterpolatedPose(path, t);
if (obj_pose) {
const auto obj_polygon = tier4_autoware_utils::toPolygon2d(*obj_pose, object.shape);
extended_object.predicted_paths.at(i).path.emplace_back(
t, *obj_pose, obj_velocity, obj_polygon);
t, *obj_pose, obj_vel_norm, obj_polygon);
}
}
}
Expand Down

0 comments on commit f7cafb1

Please sign in to comment.