Skip to content

RJD-1509 Filter nearest NPCs to calculate distance #48

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

Jump to bottom
Open
wants to merge 2 commits into
base: RJD-1509/methods_optimization
Choose a base branch
from
Open

RJD-1509 Filter nearest NPCs to calculate distance #48

wants to merge 2 commits into from

Conversation

gmajrobotec
Copy link
Collaborator

Description

This PR implements optimization of calculating actual distance in ActionNode::getDistanceToTargetEntity only for nearest NPCs.

Abstract

ActionNode::getDistanceToTargetEntity should calculate boundingBoxLaneLongitudinalDistance only for nearest NPC. This is done based on DistancesMap containg shortest distance between mapPose of NPCs.

Details

  1. Rough distance between all NPCs is calculated only once for each update in EntityManager.
  2. sharred_ptr of this map is passed to behavior_tree and used ActionNode::getDistanceToTargetEntity.
  3. Nearest NPCs are NPCs in distance of NPC spline length.

Destructive Changes

--

Known Limitations

--

References

RJD-1509

dmoszynski
dmoszynski reviewed Apr 7, 2025
View reviewed changes
dmoszynski
dmoszynski reviewed Apr 7, 2025
View reviewed changes
dmoszynski
dmoszynski reviewed Apr 7, 2025
View reviewed changes
dmoszynski
dmoszynski reviewed Apr 7, 2025
View reviewed changes
dmoszynski
dmoszynski reviewed Apr 7, 2025
View reviewed changes
dmoszynski
dmoszynski reviewed Apr 7, 2025
View reviewed changes
dmoszynski
dmoszynski reviewed Apr 7, 2025
View reviewed changes
dmoszynski
dmoszynski reviewed Apr 7, 2025
View reviewed changes
@gmajrobotec gmajrobotec force-pushed the RJD-1509/rough_distances branch from 3adec57 to ef231da Compare April 8, 2025 12:05
@gmajrobotec gmajrobotec requested a review from dmoszynski April 10, 2025 09:43
@gmajrobotec gmajrobotec force-pushed the RJD-1509/rough_distances branch from ef231da to 5b6c046 Compare April 10, 2025 10:46
dmoszynski
dmoszynski reviewed Apr 10, 2025
View reviewed changes
simulation/behavior_tree_plugin/src/action_node.cpp Outdated
}

for (const auto & [euclidean_distance, name] : euclidean_distances) {
const auto quat = math::geometry::getRotation(
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

quaternion

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done

dmoszynski
dmoszynski reviewed Apr 10, 2025
View reviewed changes
simulation/traffic_simulator/src/entity/entity_base.cpp Outdated
return false;
}

void EntityBase::setDistances(const std::shared_ptr<EuclideanDistancesMap> & distances)
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

setEuclideanDistancesMap

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done

dmoszynski
dmoszynski reviewed Apr 10, 2025
View reviewed changes
simulation/behavior_tree_plugin/src/action_node.cpp Outdated
distances.emplace_back(distance.value());
if (euclidean_distances_map != nullptr) {
std::map<double, std::string> euclidean_distances;
double stopDistance = calculateStopDistance(behavior_parameter.dynamic_constraints);
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

const double

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done

dmoszynski
dmoszynski reviewed Apr 10, 2025
View reviewed changes
simulation/behavior_tree_plugin/src/action_node.cpp Outdated
if (
std::fabs(math::geometry::convertQuaternionToEulerAngle(quat).z) <=
boost::math::constants::half_pi<double>()) {
const auto distance = getDistanceToTargetEntity(spline, other_entity_status.at(name));
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

longitudinal_distance

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done

dmoszynski
dmoszynski reviewed Apr 10, 2025
View reviewed changes
simulation/behavior_tree_plugin/src/action_node.cpp Outdated
entities.emplace_back(name);
distances.emplace_back(distance.value());
if (euclidean_distances_map != nullptr) {
std::map<double, std::string> euclidean_distances;
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

local_euclidean_distances

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

done

@gmajrobotec gmajrobotec force-pushed the RJD-1509/methods_optimization branch from c58be5a to dc503c6 Compare April 10, 2025 11:35
@gmajrobotec gmajrobotec force-pushed the RJD-1509/rough_distances branch from 5b6c046 to 49a9ef5 Compare April 10, 2025 11:36
dmoszynski
dmoszynski reviewed Apr 10, 2025
View reviewed changes
simulation/behavior_tree_plugin/src/action_node.cpp Outdated
distances.emplace_back(distance.value());
if (euclidean_distances_map != nullptr) {
std::map<double, std::string> local_euclidean_distances_map;
const double stopDistance = calculateStopDistance(behavior_parameter.dynamic_constraints);
Copy link

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

stop_distance

@gmajrobotec gmajrobotec force-pushed the RJD-1509/rough_distances branch from 49a9ef5 to 4beca20 Compare April 10, 2025 11:41
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@dmoszynski dmoszynski dmoszynski left review comments

@piotr-zyskowski-rai piotr-zyskowski-rai Awaiting requested review from piotr-zyskowski-rai

Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
2 participants
@gmajrobotec @dmoszynski