Skip to content

Commit

Permalink
refactor(bpp): simplify extended predicted object initialization
Browse files Browse the repository at this point in the history
Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
  • Loading branch information
zulfaqar-azmi-t4 committed Apr 22, 2024
1 parent fb5b20f commit c8a3adf
Show file tree
Hide file tree
Showing 3 changed files with 13 additions and 13 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -1095,12 +1095,7 @@ ExtendedPredictedObject transform(
[[maybe_unused]] const BehaviorPathPlannerParameters & common_parameters,
const LaneChangeParameters & lane_change_parameters, const bool check_at_prepare_phase)
{
ExtendedPredictedObject extended_object;
extended_object.uuid = object.object_id;
extended_object.initial_pose = object.kinematics.initial_pose_with_covariance;
extended_object.initial_twist = object.kinematics.initial_twist_with_covariance;
extended_object.initial_acceleration = object.kinematics.initial_acceleration_with_covariance;
extended_object.shape = object.shape;
ExtendedPredictedObject extended_object(object);

const auto & time_resolution = lane_change_parameters.prediction_time_resolution;
const auto & prepare_duration = lane_change_parameters.lane_change_prepare_duration;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,7 @@
namespace behavior_path_planner::utils::path_safety_checker
{

using autoware_auto_perception_msgs::msg::PredictedObject;
using geometry_msgs::msg::Pose;
using geometry_msgs::msg::Twist;
using tier4_autoware_utils::Polygon2d;
Expand Down Expand Up @@ -79,6 +80,16 @@ struct ExtendedPredictedObject
autoware_auto_perception_msgs::msg::Shape shape;
std::vector<autoware_auto_perception_msgs::msg::ObjectClassification> classification;
std::vector<PredictedPathWithPolygon> predicted_paths;

ExtendedPredictedObject() = default;
explicit ExtendedPredictedObject(const PredictedObject & object)
: uuid(object.object_id),
initial_pose(object.kinematics.initial_pose_with_covariance),
initial_twist(object.kinematics.initial_twist_with_covariance),
initial_acceleration(object.kinematics.initial_acceleration_with_covariance),
classification(object.classification)
{
}
};
using ExtendedPredictedObjects = std::vector<ExtendedPredictedObject>;

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -289,13 +289,7 @@ ExtendedPredictedObject transform(
const PredictedObject & object, const double safety_check_time_horizon,
const double safety_check_time_resolution)
{
ExtendedPredictedObject extended_object;
extended_object.uuid = object.object_id;
extended_object.initial_pose = object.kinematics.initial_pose_with_covariance;
extended_object.initial_twist = object.kinematics.initial_twist_with_covariance;
extended_object.initial_acceleration = object.kinematics.initial_acceleration_with_covariance;
extended_object.shape = object.shape;
extended_object.classification = object.classification;
ExtendedPredictedObject extended_object(object);

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

Expand Down

0 comments on commit c8a3adf

Please sign in to comment.