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.

Sign up for GitHub

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

fix(map_based_prediction): use autoware utils #547

Merged
merged 2 commits into from
Mar 18, 2022
Merged

fix(map_based_prediction): use autoware utils #547

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
5b503e8
use autoware utils
purewater0901 Mar 18, 2022
9d05a7f
add const
purewater0901 Mar 18, 2022
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
6 changes: 3 additions & 3 deletions perception/map_based_prediction/src/map_based_prediction_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -301,7 +301,7 @@ double MapBasedPredictionNode::getObjectYaw(const TrackedObject & object)
const auto & object_twist = object.kinematics.twist_with_covariance.twist;
const auto future_object_pose = tier4_autoware_utils::calcOffsetPose(
object_pose, object_twist.linear.x * 0.1, object_twist.linear.y * 0.1, 0.0);
return tier4_autoware_utils::calcAzimuthAngle(future_object_pose.position, object_pose.position);
return tier4_autoware_utils::calcAzimuthAngle(object_pose.position, future_object_pose.position);
}

void MapBasedPredictionNode::removeOldObjectsHistory(const double current_time)
Expand Down Expand Up @@ -412,11 +412,11 @@ bool MapBasedPredictionNode::checkCloseLaneletCondition(
}

// Step3. Calculate the angle difference between the lane angle and obstacle angle
double object_yaw = getObjectYaw(object);
const double object_yaw = getObjectYaw(object);
const double lane_yaw = lanelet::utils::getLaneletAngle(
lanelet.second, object.kinematics.pose_with_covariance.pose.position);
const double delta_yaw = object_yaw - lane_yaw;
const double normalized_delta_yaw = std::atan2(std::sin(delta_yaw), std::cos(delta_yaw));
const double normalized_delta_yaw = tier4_autoware_utils::normalizeRadian(delta_yaw);
const double abs_norm_delta = std::fabs(normalized_delta_yaw);

// Step4. Check if the closest lanelet is valid, and add all
Expand Down