Skip to content

Commit

Permalink
clean up
Browse files Browse the repository at this point in the history 
Signed-off-by: Yuki Takagi <[email protected]>
  • Loading branch information
@yuki-takagi-66
yuki-takagi-66 committed Apr 1, 2024
1 parent b624c37 commit e793646
Show file tree
Hide file tree
Showing 3 changed files with 74 additions and 87 deletions.
23 changes: 14 additions & 9 deletions ...or_path_dynamic_avoidance_module/include/behavior_path_dynamic_avoidance_module/scene.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,9 +78,9 @@ enum class PolygonGenerationMethod {
};

enum class ObjectBehaviorType {
OutOfTarget = 0,
RegulatedObject,
PrioritizedObject,
Ignore = 0,
Regulated,
Prioritized,
};

struct DynamicAvoidanceParameters
Expand Down Expand Up @@ -119,12 +119,18 @@ struct DynamicAvoidanceParameters
double max_overtaking_crossing_object_angle{0.0};
double min_oncoming_crossing_object_vel{0.0};
double max_oncoming_crossing_object_angle{0.0};
double max_pedestrians_crossing_vel{0.0};
double max_stopped_object_vel{0.0};

// drivable area generation
PolygonGenerationMethod polygon_generation_method{};
double min_obj_path_based_lon_polygon_margin{0.0};
double lat_offset_from_obstacle{0.0};
double margin_distance_around_pedestrian{0.0};

double end_time_to_consider{0.0};
double threshold_confidence{0.0};

double max_lat_offset_to_avoid{0.0};
double max_time_for_lat_shift{0.0};
double lpf_gain_for_lat_avoid_to_offset{0.0};
Expand Down Expand Up @@ -242,7 +248,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface

// increase counter
if (counter_map_.count(uuid) != 0) {
counter_map_.at(uuid) = std::min(max_count_, counter_map_.at(uuid) + 1);
counter_map_.at(uuid) = std::min(max_count_, std::max(1, counter_map_.at(uuid) + 1));
} else {
counter_map_.emplace(uuid, 1);
}
Expand Down Expand Up @@ -286,7 +292,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface
// remove objects whose counter is lower than threshold
const auto counter_map_keys = getAllKeys(counter_map_);
for (const auto & key : counter_map_keys) {
if (counter_map_.at(key) == 0) {
if (counter_map_.at(key) < min_count_) {
counter_map_.erase(key);
object_map_.erase(key);
// std::cerr << "delete: " << key << std::endl;
Expand Down Expand Up @@ -378,9 +384,9 @@ class DynamicAvoidanceModule : public SceneModuleInterface
bool canTransitFailureState() override { return false; }

ObjectBehaviorType getLabelAsTargetObstacle(const uint8_t label) const;
void registerLaneDriveObjects(const std::vector<DynamicAvoidanceObject> & prev_objects);
void registerRegulatedObjects(const std::vector<DynamicAvoidanceObject> & prev_objects);
void registerPrioritizedObjects(const std::vector<DynamicAvoidanceObject> & prev_objects);
void determineWhetherToAvoidAgainstLaneDriveObjects(
void determineWhetherToAvoidAgainstRegulatedObjects(
const std::vector<DynamicAvoidanceObject> & prev_objects);
void determineWhetherToAvoidAgainstPrioritizedObjects(
const std::vector<DynamicAvoidanceObject> & prev_objects);
Expand Down Expand Up @@ -432,8 +438,7 @@ class DynamicAvoidanceModule : public SceneModuleInterface
const DynamicAvoidanceObject & object) const;
std::optional<tier4_autoware_utils::Polygon2d> calcPrioritizedObstaclePolygon(
const DynamicAvoidanceObject & object, const EgoPathReservePoly & ego_path_poly) const;
EgoPathReservePoly calcEgoPathReservePoly(
const PathWithLaneId & ego_path) const;
EgoPathReservePoly calcEgoPathReservePoly(const PathWithLaneId & ego_path) const;

void printIgnoreReason(const std::string & obj_uuid, const std::string & reason)
{
Expand Down
16 changes: 16 additions & 0 deletions planning/behavior_path_dynamic_avoidance_module/src/manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -99,6 +99,8 @@ void DynamicAvoidanceModuleManager::init(rclcpp::Node * node)
node->declare_parameter<double>(ns + "crossing_object.min_oncoming_object_vel");
p.max_oncoming_crossing_object_angle =
node->declare_parameter<double>(ns + "crossing_object.max_oncoming_object_angle");
p.max_pedestrians_crossing_vel =
node->declare_parameter<double>(ns + "crossing_object.max_pedestrians_crossing_vel");

p.max_stopped_object_vel =
node->declare_parameter<double>(ns + "stopped_object.max_object_vel");
Expand All @@ -111,6 +113,11 @@ void DynamicAvoidanceModuleManager::init(rclcpp::Node * node)
p.min_obj_path_based_lon_polygon_margin =
node->declare_parameter<double>(ns + "object_path_base.min_longitudinal_polygon_margin");
p.lat_offset_from_obstacle = node->declare_parameter<double>(ns + "lat_offset_from_obstacle");
p.margin_distance_around_pedestrian =
node->declare_parameter<double>(ns + "predicted_path.margin_distance_around_pedestrian");
p.end_time_to_consider =
node->declare_parameter<double>(ns + "predicted_path.end_time_to_consider");
p.threshold_confidence = node->declare_parameter<double>(ns + "threshold_confidence");

Check warning on line 120 in planning/behavior_path_dynamic_avoidance_module/src/manager.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Large Method 

DynamicAvoidanceModuleManager::init increases from 88 to 95 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.
p.max_lat_offset_to_avoid = node->declare_parameter<double>(ns + "max_lat_offset_to_avoid");
p.max_time_for_lat_shift =
node->declare_parameter<double>(ns + "max_time_for_object_lat_shift");
Expand Down Expand Up @@ -214,6 +221,9 @@ void DynamicAvoidanceModuleManager::updateModuleParams(
updateParam<double>(
parameters, ns + "crossing_object.max_oncoming_object_angle",
p->max_oncoming_crossing_object_angle);
updateParam<double>(
parameters, ns + "crossing_object.max_pedestrians_crossing_vel",
p->max_pedestrians_crossing_vel);

updateParam<double>(
parameters, ns + "stopped_object.max_object_vel", p->max_stopped_object_vel);
Expand All @@ -231,6 +241,12 @@ void DynamicAvoidanceModuleManager::updateModuleParams(
parameters, ns + "object_path_base.min_longitudinal_polygon_margin",
p->min_obj_path_based_lon_polygon_margin);
updateParam<double>(parameters, ns + "lat_offset_from_obstacle", p->lat_offset_from_obstacle);
updateParam<double>(
parameters, ns + "margin_distance_around_pedestrian", p->margin_distance_around_pedestrian);
updateParam<double>(
parameters, ns + "predicted_path.end_time_to_consider", p->end_time_to_consider);
updateParam<double>(
parameters, ns + "predicted_path.threshold_confidence", p->threshold_confidence);

Check warning on line 249 in planning/behavior_path_dynamic_avoidance_module/src/manager.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Large Method 

DynamicAvoidanceModuleManager::updateModuleParams increases from 112 to 121 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.
updateParam<double>(parameters, ns + "max_lat_offset_to_avoid", p->max_lat_offset_to_avoid);
updateParam<double>(
parameters, ns + "max_time_for_object_lat_shift", p->max_time_for_lat_shift);
Expand Down
Loading

0 comments on commit e793646

Please sign in to comment.