Skip to content
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

fix(autoware_behavior_velocity_intersection_module): fix funcArgNamesDifferent #8023

Merged
merged 4 commits into from
Aug 1, 2024
+30 −30
Merged

fix(autoware_behavior_velocity_intersection_module): fix funcArgNamesDifferent #8023

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
088818c
fix:funcArgNamesDifferent
kobayu858 Jul 12, 2024
b6fa0e9
fix:funcArgNamesDifferent
kobayu858 Jul 12, 2024
660f851
refactor:clang format
kobayu858 Jul 12, 2024
4ad327d
fix:funcArgNamesDifferent
kobayu858 Jul 31, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
36 changes: 18 additions & 18 deletions ...or_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -74,12 +74,12 @@

void ObjectInfo::initialize(
const autoware_perception_msgs::msg::PredictedObject & object,
std::optional<lanelet::ConstLanelet> attention_lanelet_opt_,
std::optional<lanelet::ConstLineString3d> stopline_opt_)
std::optional<lanelet::ConstLanelet> attention_lanelet_opt,
std::optional<lanelet::ConstLineString3d> stopline_opt)
{
predicted_object_ = object;
attention_lanelet_opt = attention_lanelet_opt_;
stopline_opt = stopline_opt_;
attention_lanelet_opt_ = attention_lanelet_opt;
stopline_opt_ = stopline_opt;
unsafe_interval_ = std::nullopt;
calc_dist_to_stopline();
}
Expand All @@ -96,10 +96,10 @@
std::optional<geometry_msgs::msg::Point> ObjectInfo::estimated_past_position(
const double past_duration) const
{
if (!attention_lanelet_opt) {
if (!attention_lanelet_opt_) {
return std::nullopt;
}
const auto attention_lanelet = attention_lanelet_opt.value();
const auto attention_lanelet = attention_lanelet_opt_.value();
const auto current_arc_coords = lanelet::utils::getArcCoordinates(
{attention_lanelet}, predicted_object_.kinematics.initial_pose_with_covariance.pose);
const auto distance = current_arc_coords.distance;
Expand All @@ -116,29 +116,29 @@

void ObjectInfo::calc_dist_to_stopline()
{
if (!stopline_opt || !attention_lanelet_opt) {
if (!stopline_opt_ || !attention_lanelet_opt_) {
return;
}
const auto attention_lanelet = attention_lanelet_opt.value();
const auto attention_lanelet = attention_lanelet_opt_.value();
const auto object_arc_coords = lanelet::utils::getArcCoordinates(
{attention_lanelet}, predicted_object_.kinematics.initial_pose_with_covariance.pose);
const auto stopline = stopline_opt.value();
const auto stopline = stopline_opt_.value();
geometry_msgs::msg::Pose stopline_center;
stopline_center.position.x = (stopline.front().x() + stopline.back().x()) / 2.0;
stopline_center.position.y = (stopline.front().y() + stopline.back().y()) / 2.0;
stopline_center.position.z = (stopline.front().z() + stopline.back().z()) / 2.0;
const auto stopline_arc_coords =
lanelet::utils::getArcCoordinates({attention_lanelet}, stopline_center);
dist_to_stopline_opt = (stopline_arc_coords.length - object_arc_coords.length);
dist_to_stopline_opt_ = (stopline_arc_coords.length - object_arc_coords.length);
}

bool ObjectInfo::can_stop_before_stopline(const double brake_deceleration) const
{
if (!dist_to_stopline_opt) {
if (!dist_to_stopline_opt_) {
return false;
}
const double velocity = predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x;
const double dist_to_stopline = dist_to_stopline_opt.value();
const double dist_to_stopline = dist_to_stopline_opt_.value();

Check warning on line 141 in planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp#L141 

Added line #L141 was not covered by tests
const double braking_distance = (velocity * velocity) / (2.0 * brake_deceleration);
return dist_to_stopline > braking_distance;
}
Expand All @@ -147,17 +147,17 @@
const double brake_deceleration, const double tolerable_overshoot,
lanelet::ConstLanelet ego_lane) const
{
if (!dist_to_stopline_opt || !stopline_opt || !attention_lanelet_opt) {
if (!dist_to_stopline_opt_ || !stopline_opt_ || !attention_lanelet_opt_) {
return false;
}
const double dist_to_stopline = dist_to_stopline_opt.value();
const double dist_to_stopline = dist_to_stopline_opt_.value();

Check warning on line 153 in planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp#L153 

Added line #L153 was not covered by tests
const double velocity = predicted_object_.kinematics.initial_twist_with_covariance.twist.linear.x;
const double braking_distance = (velocity * velocity) / (2.0 * brake_deceleration);
if (dist_to_stopline > braking_distance) {
return false;
}
const auto attention_lanelet = attention_lanelet_opt.value();
const auto stopline = stopline_opt.value();
const auto attention_lanelet = attention_lanelet_opt_.value();
const auto stopline = stopline_opt_.value();
const auto stopline_p1 = stopline.front();
const auto stopline_p2 = stopline.back();
const autoware::universe_utils::Point2d stopline_mid{
Expand Down Expand Up @@ -185,10 +185,10 @@

bool ObjectInfo::before_stopline_by(const double margin) const
{
if (!dist_to_stopline_opt) {
if (!dist_to_stopline_opt_) {
return false;
}
const double dist_to_stopline = dist_to_stopline_opt.value();
const double dist_to_stopline = dist_to_stopline_opt_.value();

Check warning on line 191 in planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.cpp#L191 

Added line #L191 was not covered by tests
return dist_to_stopline < margin;
}

Expand Down
18 changes: 9 additions & 9 deletions ...or_velocity_planner/autoware_behavior_velocity_intersection_module/src/object_manager.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -126,17 +126,16 @@ class ObjectInfo
* @brief update predicted_object_, attention_lanelet, stopline, dist_to_stopline
*/
void initialize(
const autoware_perception_msgs::msg::PredictedObject & predicted_object,
const autoware_perception_msgs::msg::PredictedObject & object,
std::optional<lanelet::ConstLanelet> attention_lanelet_opt,
std::optional<lanelet::ConstLineString3d> stopline_opt);

/**
* @brief update unsafe_knowledge
*/
void update_safety(
const std::optional<CollisionInterval> & unsafe_interval_opt,
const std::optional<CollisionInterval> & safe_interval_opt,
const bool safe_under_traffic_control);
const std::optional<CollisionInterval> & unsafe_interval,
const std::optional<CollisionInterval> & safe_interval, const bool safe_under_traffic_control);

/**
* @brief find the estimated position of the object in the past
Expand Down Expand Up @@ -196,13 +195,13 @@ class ObjectInfo
autoware_perception_msgs::msg::PredictedObject predicted_object_;

//! null if the object in intersection_area but not in attention_area
std::optional<lanelet::ConstLanelet> attention_lanelet_opt{std::nullopt};
std::optional<lanelet::ConstLanelet> attention_lanelet_opt_{std::nullopt};

//! null if the object in intersection_area but not in attention_area
std::optional<lanelet::ConstLineString3d> stopline_opt{std::nullopt};
std::optional<lanelet::ConstLineString3d> stopline_opt_{std::nullopt};

//! null if the object in intersection_area but not in attention_area
std::optional<double> dist_to_stopline_opt{std::nullopt};
std::optional<double> dist_to_stopline_opt_{std::nullopt};

//! store the information if judged as UNSAFE
std::optional<CollisionInterval> unsafe_interval_{std::nullopt};
Expand Down Expand Up @@ -230,11 +229,12 @@ class ObjectInfoManager
public:
std::shared_ptr<ObjectInfo> registerObject(
const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area,
const bool belong_intersection_area, const bool is_parked);
const bool belong_intersection_area, const bool is_parked_vehicle);

void registerExistingObject(
const unique_identifier_msgs::msg::UUID & uuid, const bool belong_attention_area,
const bool belong_intersection_area, const bool is_parked, std::shared_ptr<ObjectInfo> object);
const bool belong_intersection_area, const bool is_parked_vehicle,
std::shared_ptr<ObjectInfo> object);

void clearObjects();

Expand Down
6 changes: 3 additions & 3 deletions ...elocity_planner/autoware_behavior_velocity_intersection_module/src/scene_intersection.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -590,7 +590,7 @@ class IntersectionModule : public SceneModuleInterface
* @brief generate discretized detection lane linestring.
*/
std::vector<lanelet::ConstLineString3d> generateDetectionLaneDivisions(
lanelet::ConstLanelets detection_lanelets,
lanelet::ConstLanelets detection_lanelets_all,
const lanelet::routing::RoutingGraphPtr routing_graph_ptr, const double resolution) const;
/** @} */

Expand Down Expand Up @@ -752,7 +752,7 @@ class IntersectionModule : public SceneModuleInterface

void cutPredictPathWithinDuration(
const builtin_interfaces::msg::Time & object_stamp, const double time_thr,
autoware_perception_msgs::msg::PredictedPath * predicted_path) const;
autoware_perception_msgs::msg::PredictedPath * path) const;

/**
* @brief check if there are any objects around the stoplines on the attention areas when ego
Expand Down Expand Up @@ -809,7 +809,7 @@ class IntersectionModule : public SceneModuleInterface
TimeDistanceArray calcIntersectionPassingTime(
const tier4_planning_msgs::msg::PathWithLaneId & path, const bool is_prioritized,
const IntersectionStopLines & intersection_stoplines,
tier4_debug_msgs::msg::Float64MultiArrayStamped * debug_ttc_array) const;
tier4_debug_msgs::msg::Float64MultiArrayStamped * ego_ttc_array) const;
/** @} */

mutable DebugData debug_data_;
Expand Down
Loading