Skip to content

Commit

Permalink
update
Browse files Browse the repository at this point in the history 
Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
  • Loading branch information
@takayuki5168
takayuki5168 committed Aug 19, 2023
1 parent aa0ec61 commit dde50c6
Show file tree
Hide file tree
Showing 4 changed files with 22 additions and 18 deletions.
13 changes: 8 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,17 +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 = std::hypot(
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.x,
object.kinematics.initial_twist_with_covariance.twist.linear.y);
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 @@ -367,7 +370,7 @@ 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 = std::hypot(
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);
Expand Down Expand Up @@ -397,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
4 changes: 2 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,7 @@ 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 = std::hypot(
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 =
Expand All @@ -702,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
8 changes: 4 additions & 4 deletions planning/behavior_path_planner/src/utils/avoidance/utils.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -694,8 +694,8 @@ void fillObjectMovingTime(
const auto object_parameter = parameters->object_parameters.at(object_type);

const auto & object_twist = object_data.object.kinematics.initial_twist_with_covariance.twist;
const auto object_vel = std::hypot(object_twist.linear.x, object_twist.linear.y);
const auto is_faster_than_threshold = object_vel > object_parameter.moving_speed_threshold;
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,7 @@ ExtendedPredictedObject transform(
extended_object.initial_acceleration = object.kinematics.initial_acceleration_with_covariance;
extended_object.shape = object.shape;

const auto & obj_velocity = std::hypot(
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 @@ -1491,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
15 changes: 8 additions & 7 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,9 +855,9 @@ bool isParkedObject(
using lanelet::geometry::distance2d;
using lanelet::geometry::toArcCoordinates;

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

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

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

Expand Down Expand Up @@ -1100,7 +1100,7 @@ ExtendedPredictedObject transform(
const auto & velocity_threshold =
lane_change_parameters.prepare_segment_ignore_object_velocity_thresh;
const auto start_time = check_at_prepare_phase ? 0.0 : prepare_duration;
const double obj_vel = std::hypot(
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());
Expand All @@ -1113,13 +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_vel, obj_polygon);
extended_object.predicted_paths.at(i).path.emplace_back(
t, *obj_pose, obj_vel_norm, obj_polygon);
}
}
}
Expand Down

0 comments on commit dde50c6

Please sign in to comment.