Skip to content

Commit 5a6cde9

Browse files
refactor(bpp): simplify extended predicted object initialization (#6858)
* refactor(bpp): simplify extended predicted object initialization Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> * Update planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com> --------- Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp> Co-authored-by: Maxime CLEMENT <78338830+maxime-clem@users.noreply.github.com>
1 parent 702b183 commit 5a6cde9

File tree

3 files changed

+14
-13
lines changed

3 files changed

+14
-13
lines changed

planning/behavior_path_lane_change_module/src/utils/utils.cpp

+1-6
Original file line numberDiff line numberDiff line change
@@ -1112,12 +1112,7 @@ ExtendedPredictedObject transform(
11121112
[[maybe_unused]] const BehaviorPathPlannerParameters & common_parameters,
11131113
const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase)
11141114
{
1115-
ExtendedPredictedObject extended_object;
1116-
extended_object.uuid = object.object_id;
1117-
extended_object.initial_pose = object.kinematics.initial_pose_with_covariance;
1118-
extended_object.initial_twist = object.kinematics.initial_twist_with_covariance;
1119-
extended_object.initial_acceleration = object.kinematics.initial_acceleration_with_covariance;
1120-
extended_object.shape = object.shape;
1115+
ExtendedPredictedObject extended_object(object);
11211116

11221117
const auto & time_resolution = lane_change_parameters.prediction_time_resolution;
11231118
const auto & prepare_duration = lane_change_parameters.lane_change_prepare_duration;

planning/behavior_path_planner_common/include/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp

+12
Original file line numberDiff line numberDiff line change
@@ -31,6 +31,7 @@
3131
namespace behavior_path_planner::utils::path_safety_checker
3232
{
3333

34+
using autoware_auto_perception_msgs::msg::PredictedObject;
3435
using geometry_msgs::msg::Pose;
3536
using geometry_msgs::msg::Twist;
3637
using tier4_autoware_utils::Polygon2d;
@@ -79,6 +80,17 @@ struct ExtendedPredictedObject
7980
autoware_auto_perception_msgs::msg::Shape shape;
8081
std::vector<autoware_auto_perception_msgs::msg::ObjectClassification> classification;
8182
std::vector<PredictedPathWithPolygon> predicted_paths;
83+
84+
ExtendedPredictedObject() = default;
85+
explicit ExtendedPredictedObject(const PredictedObject & object)
86+
: uuid(object.object_id),
87+
initial_pose(object.kinematics.initial_pose_with_covariance),
88+
initial_twist(object.kinematics.initial_twist_with_covariance),
89+
initial_acceleration(object.kinematics.initial_acceleration_with_covariance),
90+
shape(object.shape),
91+
classification(object.classification)
92+
{
93+
}
8294
};
8395
using ExtendedPredictedObjects = std::vector<ExtendedPredictedObject>;
8496

planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp

+1-7
Original file line numberDiff line numberDiff line change
@@ -289,13 +289,7 @@ ExtendedPredictedObject transform(
289289
const PredictedObject & object, const double safety_check_time_horizon,
290290
const double safety_check_time_resolution)
291291
{
292-
ExtendedPredictedObject extended_object;
293-
extended_object.uuid = object.object_id;
294-
extended_object.initial_pose = object.kinematics.initial_pose_with_covariance;
295-
extended_object.initial_twist = object.kinematics.initial_twist_with_covariance;
296-
extended_object.initial_acceleration = object.kinematics.initial_acceleration_with_covariance;
297-
extended_object.shape = object.shape;
298-
extended_object.classification = object.classification;
292+
ExtendedPredictedObject extended_object(object);
299293

300294
const auto obj_velocity = extended_object.initial_twist.twist.linear.x;
301295

0 commit comments

Comments
 (0)