From eec09a70f8f49b2503715b49e11396d8e26352af Mon Sep 17 00:00:00 2001 From: mitukou1109 Date: Thu, 27 Jun 2024 11:01:58 +0900 Subject: [PATCH] reduce diff --- .../autoware/obstacle_cruise_planner/node.hpp | 2 +- .../autoware_obstacle_cruise_planner/src/node.cpp | 15 ++++++--------- 2 files changed, 7 insertions(+), 10 deletions(-) diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp index 908b72744c8e6..826f14ba410f2 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp @@ -123,7 +123,6 @@ class ObstacleCruisePlannerNode : public rclcpp::Node bool enable_debug_info_; bool enable_calculation_time_info_; - bool enable_slow_down_planning_{false}; bool use_pointcloud_for_stop_; bool use_pointcloud_for_slow_down_; double min_behavior_stop_margin_; @@ -292,6 +291,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node EgoNearestParam ego_nearest_param_; bool is_driving_forward_{true}; + bool enable_slow_down_planning_{false}; bool use_pointcloud_{false}; std::vector prev_closest_stop_obstacles_{}; diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 22b58f4584d4f..3ee92f9bfd965 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -558,8 +558,8 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms } const auto & ego_odom = *ego_odom_ptr; - const auto & acc = *acc_ptr; const auto & objects = objects_ptr ? std::make_optional(*objects_ptr) : std::nullopt; + const auto & acc = *acc_ptr; const auto traj_points = autoware::motion_utils::convertToTrajectoryPointArray(*msg); // check if subscribed variables are ready @@ -1073,9 +1073,8 @@ std::optional ObstacleCruisePlannerNode::createCruiseObstacle( return std::nullopt; } - const auto & p = behavior_determination_param_; - const auto & object_id = obstacle.uuid.substr(0, 4); + const auto & p = behavior_determination_param_; // NOTE: When driving backward, Stop will be planned instead of cruise. // When the obstacle is crossing the ego's trajectory, cruise can be ignored. @@ -1121,11 +1120,10 @@ std::optional ObstacleCruisePlannerNode::createYieldCruiseObstac return std::nullopt; } - const auto & p = behavior_determination_param_; - if (traj_points.empty()) return std::nullopt; // check label const auto & object_id = obstacle.uuid.substr(0, 4); + const auto & p = behavior_determination_param_; if (!isOutsideCruiseObstacle(obstacle.classification.label)) { RCLCPP_INFO_EXPRESSION( @@ -1409,10 +1407,9 @@ std::optional ObstacleCruisePlannerNode::createStopObstacle( std::optional> collision_point = std::nullopt; if (obstacle.source_type == Obstacle::SourceType::POINTCLOUD) { if (obstacle.stop_collision_point) { - const auto dist_to_collide_on_traj = autoware::motion_utils::calcSignedArcLength( - traj_points, 0, obstacle.stop_collision_point.value()); - collision_point = - std::make_pair(obstacle.stop_collision_point.value(), dist_to_collide_on_traj); + const auto dist_to_collide_on_traj = + autoware::motion_utils::calcSignedArcLength(traj_points, 0, *obstacle.stop_collision_point); + collision_point = std::make_pair(*obstacle.stop_collision_point, dist_to_collide_on_traj); } } else if (obstacle.source_type == Obstacle::SourceType::PREDICTED_OBJECT) { const auto & object_id = obstacle.uuid.substr(0, 4);