From 08c2505a71dde869798c0f90988cfd92b3bcf3be Mon Sep 17 00:00:00 2001 From: Mateusz Palczuk Date: Thu, 19 Dec 2024 10:38:18 +0100 Subject: [PATCH] Refactor: remove duplicated data: behavior_parameter Signed-off-by: Mateusz Palczuk --- .../follow_polyline_trajectory_action.cpp | 2 +- .../follow_polyline_trajectory_action.cpp | 2 +- .../polyline_trajectory_follower.hpp | 2 -- .../polyline_trajectory_positioner.hpp | 3 --- .../polyline_trajectory_follower.cpp | 10 ++++------ .../polyline_trajectory_positioner.cpp | 2 -- simulation/traffic_simulator/src/entity/ego_entity.cpp | 2 +- 7 files changed, 7 insertions(+), 16 deletions(-) diff --git a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp index 7f92854600a..9eac0b9155b 100644 --- a/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp +++ b/simulation/behavior_tree_plugin/src/pedestrian/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp @@ -79,7 +79,7 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus traffic_simulator::follow_trajectory::ValidatedEntityStatus( static_cast(*canonicalized_entity_status), behavior_parameter, step_time), - hdmap_utils, behavior_parameter, *polyline_trajectory, + hdmap_utils, *polyline_trajectory, default_matching_distance_for_lanelet_pose_calculation, getTargetSpeed(), step_time); entity_status_updated.has_value()) { diff --git a/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp b/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp index 20d14cb03a7..b1c2a3d77da 100644 --- a/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp +++ b/simulation/behavior_tree_plugin/src/vehicle/follow_trajectory_sequence/follow_polyline_trajectory_action.cpp @@ -79,7 +79,7 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus traffic_simulator::follow_trajectory::ValidatedEntityStatus( static_cast(*canonicalized_entity_status), behavior_parameter, step_time), - hdmap_utils, behavior_parameter, *polyline_trajectory, + hdmap_utils, *polyline_trajectory, default_matching_distance_for_lanelet_pose_calculation, getTargetSpeed(), step_time); entity_status_updated.has_value()) { diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_follower.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_follower.hpp index 0f8c8ba7bea..f7fc16e0123 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_follower.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_follower.hpp @@ -21,7 +21,6 @@ #include #include #include -#include #include #include @@ -37,7 +36,6 @@ struct PolylineTrajectoryFollower static auto makeUpdatedEntityStatus( const ValidatedEntityStatus & validated_entity_status, const std::shared_ptr & hdmap_utils_ptr, - const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const double matching_distance, const std::optional target_speed, const double step_time) -> std::optional; diff --git a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.hpp b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.hpp index bbe5b9f2a9a..876d86a8700 100644 --- a/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.hpp +++ b/simulation/traffic_simulator/include/traffic_simulator/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.hpp @@ -20,7 +20,6 @@ #include #include #include -#include #include #include @@ -36,7 +35,6 @@ struct PolylineTrajectoryPositioner const std::shared_ptr & hdmap_utils_ptr, const ValidatedEntityStatus & validated_entity_status, const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, const std::optional target_speed, const double matching_distance, const double step_time); @@ -45,7 +43,6 @@ struct PolylineTrajectoryPositioner private: const std::shared_ptr hdmap_utils_ptr; const ValidatedEntityStatus validated_entity_status; - const traffic_simulator_msgs::msg::BehaviorParameter behavior_parameter; const traffic_simulator_msgs::msg::PolylineTrajectory polyline_trajectory; const double step_time; const double matching_distance; diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp index 56538a0cd0d..23eee90ed02 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_follower.cpp @@ -35,18 +35,16 @@ namespace follow_trajectory auto PolylineTrajectoryFollower::makeUpdatedEntityStatus( const ValidatedEntityStatus & validated_entity_status, const std::shared_ptr & hdmap_utils_ptr, - const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, const double matching_distance, const std::optional target_speed, const double step_time) -> std::optional { assert(step_time > 0.0); while (not polyline_trajectory.shape.vertices.empty()) { - const auto updated_entity_opt = - PolylineTrajectoryPositioner( - hdmap_utils_ptr, validated_entity_status, polyline_trajectory, behavior_parameter, - target_speed, matching_distance, step_time) - .makeUpdatedEntityStatus(); + const auto updated_entity_opt = PolylineTrajectoryPositioner( + hdmap_utils_ptr, validated_entity_status, polyline_trajectory, + target_speed, matching_distance, step_time) + .makeUpdatedEntityStatus(); if (updated_entity_opt.has_value()) { return updated_entity_opt; } else { diff --git a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp index 1d067f30116..7f689e57b91 100644 --- a/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp +++ b/simulation/traffic_simulator/src/behavior/polyline_trajectory_follower/polyline_trajectory_positioner.cpp @@ -36,11 +36,9 @@ PolylineTrajectoryPositioner::PolylineTrajectoryPositioner( const std::shared_ptr & hdmap_utils_ptr, const ValidatedEntityStatus & validated_entity_status, const traffic_simulator_msgs::msg::PolylineTrajectory & polyline_trajectory, - const traffic_simulator_msgs::msg::BehaviorParameter & behavior_parameter, const std::optional target_speed, const double matching_distance, const double step_time) : hdmap_utils_ptr(hdmap_utils_ptr), validated_entity_status(validated_entity_status), - behavior_parameter(behavior_parameter), polyline_trajectory(polyline_trajectory), step_time(step_time), matching_distance(matching_distance), diff --git a/simulation/traffic_simulator/src/entity/ego_entity.cpp b/simulation/traffic_simulator/src/entity/ego_entity.cpp index bb855b3948a..77a8257265f 100644 --- a/simulation/traffic_simulator/src/entity/ego_entity.cpp +++ b/simulation/traffic_simulator/src/entity/ego_entity.cpp @@ -159,7 +159,7 @@ void EgoEntity::onUpdate(double current_time, double step_time) traffic_simulator::follow_trajectory::ValidatedEntityStatus( static_cast(*status_), behavior_parameter_, step_time), - hdmap_utils_ptr_, behavior_parameter_, *polyline_trajectory_, + hdmap_utils_ptr_, *polyline_trajectory_, getDefaultMatchingDistanceForLaneletPoseCalculation(), getTargetSpeed(), step_time); non_canonicalized_updated_status.has_value()) { // prefer current lanelet on ss2 side