Skip to content

Commit

Permalink
style(pre-commit): autofix
Browse files Browse the repository at this point in the history
  • Loading branch information
pre-commit-ci[bot] authored and beyzanurkaya committed Aug 26, 2024
1 parent 59b46ca commit 0776b68
Showing 1 changed file with 9 additions and 8 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -13,14 +13,14 @@
// limitations under the License.

#include "autoware/behavior_path_dynamic_obstacle_avoidance_module/scene.hpp"
#include <autoware/universe_utils/geometry/geometry.hpp>

#include "autoware/behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp"
#include "autoware/behavior_path_planner_common/utils/utils.hpp"
#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp"
#include "object_recognition_utils/predicted_path_utils.hpp"
#include "signal_processing/lowpass_filter_1d.hpp"

#include <autoware/universe_utils/geometry/geometry.hpp>
#include <autoware_lanelet2_extension/utility/utilities.hpp>

#include <boost/geometry/algorithms/buffer.hpp>
Expand Down Expand Up @@ -105,7 +105,7 @@ bool isEndPointsConnected(
{
const auto & left_back_point_2d = right_lane.leftBound2d().back().basicPoint();
const auto & right_back_point_2d = left_lane.rightBound2d().back().basicPoint();

constexpr double epsilon = 1e-5;
return (right_back_point_2d - left_back_point_2d).norm() < epsilon;
}
Expand Down Expand Up @@ -429,7 +429,9 @@ bool DynamicObstacleAvoidanceModule::canTransitSuccessState()

constexpr double THRESHOLD = 2.0;
const auto is_further_than_threshold =
autoware::universe_utils::calcDistance2d(getEgoPose(), autoware::universe_utils::getPose(data.reference_path.points.back())) > THRESHOLD;
autoware::universe_utils::calcDistance2d(
getEgoPose(), autoware::universe_utils::getPose(data.reference_path.points.back())) >
THRESHOLD;
if (is_further_than_threshold && is_arrived) {
RCLCPP_WARN(getLogger(), "Reach path end point. Exit.");
return true;
Expand All @@ -452,8 +454,8 @@ BehaviorModuleOutput DynamicObstacleAvoidanceModule::plan()
std::vector<DrivableAreaInfo::Obstacle> obstacles_for_drivable_area;

// generate drivable lanes
auto current_lanelets = getCurrentLanesFromPath(
getPreviousModuleOutput().reference_path, planner_data_);
auto current_lanelets =
getCurrentLanesFromPath(getPreviousModuleOutput().reference_path, planner_data_);
DrivableAreaInfo current_drivable_area_info;
BehaviorModuleOutput output;
output.path = input_path;
Expand All @@ -467,8 +469,7 @@ BehaviorModuleOutput DynamicObstacleAvoidanceModule::plan()
// generate drivable lanes
std::for_each(current_lanelets.begin(), current_lanelets.end(), [&](const auto & lanelet) {
current_drivable_area_info.drivable_lanes.push_back(
generateExpandedDrivableLanes(
lanelet, planner_data_, parameters_));
generateExpandedDrivableLanes(lanelet, planner_data_, parameters_));
});
// expand hatched road markings
current_drivable_area_info.enable_expanding_hatched_road_markings =
Expand Down Expand Up @@ -2007,7 +2008,7 @@ lanelet::ConstLanelets DynamicObstacleAvoidanceModule::getCurrentLanesFromPath(
if (path.points.empty()) {
throw std::logic_error("empty path.");
}

const auto idx = planner_data->findEgoIndex(path.points);

if (path.points.at(idx).lane_ids.empty()) {
Expand Down

0 comments on commit 0776b68

Please sign in to comment.