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

fix(obstacle stop/slow_down): early return without point cloud #10289

Merged
merged 2 commits into from
Mar 18, 2025
Merged

fix(obstacle stop/slow_down): early return without point cloud #10289

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
b1aab08
fix(obstacle stop/slow_down): early return without point cloud
takayuki5168 Mar 17, 2025
5d79a8e
update maintainer
takayuki5168 Mar 17, 2025
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
1 change: 1 addition & 0 deletions ...ng/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
<maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
<maintainer email="yuki.takagi@tier4.jp">Yuki Takagi</maintainer>
<maintainer email="maxime.clement@tier4.jp">Maxime Clement</maintainer>
<maintainer email="arjun.ram@tier4.jp">Arjun Ram</maintainer>

<license>Apache License 2.0</license>

Expand Down
15 changes: 6 additions & 9 deletions ...nner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2025 TIER IV, Inc. All rights reserved.

Check notice on line 1 in planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Overall Code Complexity 

The mean cyclomatic complexity decreases from 5.35 to 5.26, threshold = 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -454,6 +454,10 @@
{
autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_);

if (!obstacle_filtering_param_.use_pointcloud) {
return std::vector<SlowDownObstacle>{};

Check warning on line 458 in planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.cpp#L458 

Added line #L458 was not covered by tests
}

// calculate collision points with trajectory with lateral stop margin
// NOTE: For additional margin, hysteresis is not divided by two.
const auto & p = obstacle_filtering_param_;
Expand All @@ -480,11 +484,7 @@
const auto slow_down_obstacle = create_slow_down_obstacle_for_point_cloud(
rclcpp::Time(point_cloud.pointcloud.header.stamp), front_collision_point,
back_collision_point, slow_down_point_data.lat_dist_to_traj);

if (slow_down_obstacle) {
slow_down_obstacles.push_back(*slow_down_obstacle);
continue;
}
slow_down_obstacles.push_back(slow_down_obstacle);

Check warning on line 487 in planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.cpp#L487 

Added line #L487 was not covered by tests
}

RCLCPP_DEBUG(
Expand Down Expand Up @@ -630,13 +630,10 @@
back_collision_point};
}

std::optional<SlowDownObstacle> ObstacleSlowDownModule::create_slow_down_obstacle_for_point_cloud(
SlowDownObstacle ObstacleSlowDownModule::create_slow_down_obstacle_for_point_cloud(

Check warning on line 633 in planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.cpp#L633 

Added line #L633 was not covered by tests
const rclcpp::Time & stamp, const geometry_msgs::msg::Point & front_collision_point,
const geometry_msgs::msg::Point & back_collision_point, const double lat_dist_to_traj)
{
if (!obstacle_filtering_param_.use_pointcloud) {
return std::nullopt;
}
const unique_identifier_msgs::msg::UUID obj_uuid;
const auto & obj_uuid_str = autoware_utils::to_hex_string(obj_uuid);

Expand Down
2 changes: 1 addition & 1 deletion ...nner/autoware_motion_velocity_obstacle_slow_down_module/src/obstacle_slow_down_module.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -111,7 +111,7 @@ class ObstacleSlowDownModule : public PluginModuleInterface
const std::vector<Polygon2d> & decimated_traj_polys_with_lat_margin,
const std::shared_ptr<PlannerData::Object> object, const rclcpp::Time & predicted_objects_stamp,
const double dist_from_obj_poly_to_traj_poly);
std::optional<SlowDownObstacle> create_slow_down_obstacle_for_point_cloud(
SlowDownObstacle create_slow_down_obstacle_for_point_cloud(
const rclcpp::Time & stamp, const geometry_msgs::msg::Point & front_collision_point,
const geometry_msgs::msg::Point & back_collision_point, const double lat_dist_to_traj);
std::vector<SlowdownInterval> plan_slow_down(
Expand Down
1 change: 1 addition & 0 deletions planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@
<maintainer email="takayuki.murooka@tier4.jp">Takayuki Murooka</maintainer>
<maintainer email="yuki.takagi@tier4.jp">Yuki Takagi</maintainer>
<maintainer email="maxime.clement@tier4.jp">Maxime Clement</maintainer>
<maintainer email="arjun.ram@tier4.jp">Arjun Ram</maintainer>
<maintainer email="berkay@leodrive.ai">Berkay Karaman</maintainer>

<license>Apache License 2.0</license>
Expand Down
17 changes: 6 additions & 11 deletions ...locity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2025 TIER IV, Inc. All rights reserved.

Check notice on line 1 in planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Lines of Code in a Single File 

The lines of code decreases from 1053 to 1048, improve code health by reducing it to 1000. The number of Lines of Code in a single file. More Lines of Code lowers the code health.

Check notice on line 1 in planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Overall Code Complexity 

The mean cyclomatic complexity decreases from 4.26 to 4.20, threshold = 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -149,8 +149,6 @@
common_param_ = CommonParam(node);
stop_planning_param_ = StopPlanningParam(node, common_param_);
obstacle_filtering_param_ = ObstacleFilteringParam(node);
use_pointcloud_ =
get_or_declare_parameter<bool>(node, "obstacle_stop.obstacle_filtering.object_type.pointcloud");
Comment on lines 151 to -153
Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Note: The parameter was duplicated.


// common publisher
processing_time_publisher_ =
Expand Down Expand Up @@ -332,14 +330,10 @@
return stop_collision_points;
}

std::optional<StopObstacle> ObstacleStopModule::create_stop_obstacle_for_point_cloud(
StopObstacle ObstacleStopModule::create_stop_obstacle_for_point_cloud(

Check warning on line 333 in planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp#L333 

Added line #L333 was not covered by tests
const std::vector<TrajectoryPoint> & traj_points, const rclcpp::Time & stamp,
const geometry_msgs::msg::Point & stop_point, const double dist_to_bumper) const
{
if (!use_pointcloud_) {
return std::nullopt;
}

const auto dist_to_collide_on_traj =
autoware::motion_utils::calcSignedArcLength(traj_points, 0, stop_point) - dist_to_bumper;

Expand Down Expand Up @@ -448,6 +442,10 @@
{
autoware_utils::ScopedTimeTrack st(__func__, *time_keeper_);

if (!obstacle_filtering_param_.use_pointcloud) {
return std::vector<StopObstacle>{};

Check warning on line 446 in planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp#L446 

Added line #L446 was not covered by tests
}

const auto & tp = trajectory_polygon_collision_check;

const std::vector<geometry_msgs::msg::Point> stop_points =
Expand All @@ -468,10 +466,7 @@
// Filter obstacles for stop
const auto stop_obstacle = create_stop_obstacle_for_point_cloud(
decimated_traj_points, stop_obstacle_stamp, stop_point, dist_to_bumper);
if (stop_obstacle) {
stop_obstacles.push_back(*stop_obstacle);
continue;
}
stop_obstacles.push_back(stop_obstacle);

Check notice on line 469 in planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ No longer an issue: Bumpy Road Ahead 

ObstacleStopModule::filter_stop_obstacle_for_point_cloud is no longer above the threshold for logical blocks with deeply nested code. The Bumpy Road code smell is a function that contains multiple chunks of nested conditional logic. The deeper the nesting and the more bumps, the lower the code health.

Check warning on line 469 in planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.cpp#L469 

Added line #L469 was not covered by tests
}

std::vector<StopObstacle> past_stop_obstacles;
Expand Down
3 changes: 1 addition & 2 deletions ...locity_planner/autoware_motion_velocity_obstacle_stop_module/src/obstacle_stop_module.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,6 @@ class ObstacleStopModule : public PluginModuleInterface
// ros parameters
bool ignore_crossing_obstacle_{};
bool suppress_sudden_stop_{};
bool use_pointcloud_{false};
CommonParam common_param_{};
StopPlanningParam stop_planning_param_{};
ObstacleFilteringParam obstacle_filtering_param_{};
Expand Down Expand Up @@ -187,7 +186,7 @@ class ObstacleStopModule : public PluginModuleInterface
const VehicleInfo & vehicle_info, const double dist_to_bumper,
const TrajectoryPolygonCollisionCheck & trajectory_polygon_collision_check) const;

std::optional<StopObstacle> create_stop_obstacle_for_point_cloud(
StopObstacle create_stop_obstacle_for_point_cloud(
const std::vector<TrajectoryPoint> & traj_points, const rclcpp::Time & stamp,
const geometry_msgs::msg::Point & stop_point, const double dist_to_bumper) const;

Expand Down
2 changes: 0 additions & 2 deletions .../motion_velocity_planner/autoware_motion_velocity_obstacle_stop_module/src/parameters.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -120,8 +120,6 @@ struct ObstacleFilteringParam
utils::get_target_object_type(node, "obstacle_stop.obstacle_filtering.object_type.inside.");
outside_stop_object_types =
utils::get_target_object_type(node, "obstacle_stop.obstacle_filtering.object_type.outside.");
use_pointcloud = get_or_declare_parameter<bool>(
node, "obstacle_stop.obstacle_filtering.object_type.pointcloud");

obstacle_velocity_threshold_to_stop = get_or_declare_parameter<double>(
node, "obstacle_stop.obstacle_filtering.obstacle_velocity_threshold_to_stop");
Expand Down
Loading