Skip to content

Commit

Permalink
Refactor: remove duplicated data: behavior_parameter
Browse files Browse the repository at this point in the history
Signed-off-by: Mateusz Palczuk <[email protected]>
  • Loading branch information
TauTheLepton committed Dec 19, 2024
1 parent 96318b4 commit 08c2505
Show file tree
Hide file tree
Showing 7 changed files with 7 additions and 16 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -79,7 +79,7 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
traffic_simulator::follow_trajectory::ValidatedEntityStatus(
static_cast<traffic_simulator::EntityStatus>(*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()) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -79,7 +79,7 @@ auto FollowPolylineTrajectoryAction::tick() -> BT::NodeStatus
traffic_simulator::follow_trajectory::ValidatedEntityStatus(
static_cast<traffic_simulator::EntityStatus>(*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()) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -21,7 +21,6 @@
#include <traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp>
#include <traffic_simulator/data_type/entity_status.hpp>
#include <traffic_simulator/hdmap_utils/hdmap_utils.hpp>
#include <traffic_simulator_msgs/msg/behavior_parameter.hpp>
#include <traffic_simulator_msgs/msg/entity_status.hpp>
#include <traffic_simulator_msgs/msg/polyline_trajectory.hpp>

Expand All @@ -37,7 +36,6 @@ struct PolylineTrajectoryFollower
static auto makeUpdatedEntityStatus(
const ValidatedEntityStatus & validated_entity_status,
const std::shared_ptr<hdmap_utils::HdMapUtils> & 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<double> target_speed,
const double step_time) -> std::optional<EntityStatus>;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -20,7 +20,6 @@
#include <traffic_simulator/behavior/polyline_trajectory_follower/validated_entity_status.hpp>
#include <traffic_simulator/data_type/entity_status.hpp>
#include <traffic_simulator/hdmap_utils/hdmap_utils.hpp>
#include <traffic_simulator_msgs/msg/behavior_parameter.hpp>
#include <traffic_simulator_msgs/msg/entity_status.hpp>
#include <traffic_simulator_msgs/msg/polyline_trajectory.hpp>

Expand All @@ -36,7 +35,6 @@ struct PolylineTrajectoryPositioner
const std::shared_ptr<hdmap_utils::HdMapUtils> & 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<double> target_speed, const double matching_distance,
const double step_time);

Expand All @@ -45,7 +43,6 @@ struct PolylineTrajectoryPositioner
private:
const std::shared_ptr<hdmap_utils::HdMapUtils> 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;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -35,18 +35,16 @@ namespace follow_trajectory
auto PolylineTrajectoryFollower::makeUpdatedEntityStatus(
const ValidatedEntityStatus & validated_entity_status,
const std::shared_ptr<hdmap_utils::HdMapUtils> & 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<double> target_speed, const double step_time)
-> std::optional<EntityStatus>
{
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 {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -36,11 +36,9 @@ PolylineTrajectoryPositioner::PolylineTrajectoryPositioner(
const std::shared_ptr<hdmap_utils::HdMapUtils> & 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<double> 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),
Expand Down
2 changes: 1 addition & 1 deletion simulation/traffic_simulator/src/entity/ego_entity.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -159,7 +159,7 @@ void EgoEntity::onUpdate(double current_time, double step_time)
traffic_simulator::follow_trajectory::ValidatedEntityStatus(
static_cast<traffic_simulator::EntityStatus>(*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
Expand Down

0 comments on commit 08c2505

Please sign in to comment.