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_planner): change the nearest_collision_point calculation place #5794

Merged
merged 1 commit into from
Feb 16, 2024
Merged

fix(obstacle_stop_planner): change the nearest_collision_point calculation place #5794

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
37 changes: 33 additions & 4 deletions planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -107,17 +107,33 @@

struct PredictedObjectWithDetectionTime
{
explicit PredictedObjectWithDetectionTime(
const rclcpp::Time & t, geometry_msgs::msg::Point & p, PredictedObject obj)
: detection_time(t), point(p), object(std::move(obj))
explicit PredictedObjectWithDetectionTime(const rclcpp::Time & t, PredictedObject obj)
: detection_time(t), object(std::move(obj))
{
}

rclcpp::Time detection_time;
geometry_msgs::msg::Point point;
PredictedObject object;
};

struct IntersectedPredictedObject
{
explicit IntersectedPredictedObject(

Check warning on line 121 in planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp

View check run for this annotation

Codecov / codecov/patch

planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp#L121 

Added line #L121 was not covered by tests
const rclcpp::Time & t, PredictedObject obj, const Polygon2d obj_polygon,
const Polygon2d ego_polygon)
: detection_time(t),
object(std::move(obj)),

Check warning on line 125 in planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp

View check run for this annotation

Codecov / codecov/patch

planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp#L124-L125 

Added lines #L124 - L125 were not covered by tests
object_polygon{obj_polygon},
vehicle_polygon{ego_polygon}
{
}

Check warning on line 129 in planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp

View check run for this annotation

Codecov / codecov/patch

planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp#L129 

Added line #L129 was not covered by tests

rclcpp::Time detection_time;
PredictedObject object;
Polygon2d object_polygon;
Polygon2d vehicle_polygon;
};

class ObstacleStopPlannerNode : public rclcpp::Node
{
public:
Expand Down Expand Up @@ -274,6 +290,19 @@
}
}

void addPredictedObstacleToHistory(const PredictedObject & obj, const rclcpp::Time & now)

Check warning on line 293 in planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp

View check run for this annotation

Codecov / codecov/patch

planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp#L293 

Added line #L293 was not covered by tests
{
for (auto itr = predicted_object_history_.begin(); itr != predicted_object_history_.end();) {
if (obj.object_id.uuid == itr->object.object_id.uuid) {
// Erase the itr from the vector
itr = predicted_object_history_.erase(itr);

Check warning on line 298 in planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp

View check run for this annotation

Codecov / codecov/patch

planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp#L298 

Added line #L298 was not covered by tests
} else {
++itr;
}
}
predicted_object_history_.emplace_back(now, obj);
}

Check warning on line 304 in planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp

View check run for this annotation

Codecov / codecov/patch

planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp#L303-L304 

Added lines #L303 - L304 were not covered by tests

PointCloud::Ptr getOldPointCloudPtr() const
{
PointCloud::Ptr ret(new PointCloud);
Expand Down
170 changes: 81 additions & 89 deletions planning/obstacle_stop_planner/src/node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2020 Tier IV, Inc.

Check notice on line 1 in planning/obstacle_stop_planner/src/node.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 1257 to 1248, 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/obstacle_stop_planner/src/node.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 8.81 to 8.62, 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 @@ -749,219 +749,209 @@
}

{
double min_collision_norm = 0.0;
bool is_init = false;
size_t nearest_collision_object_index = 0;
geometry_msgs::msg::Point nearest_collision_point;

bool collision_found_on_trajectory_point = false;
for (size_t j = 0; j < filtered_objects.objects.size(); ++j) {
const auto & obj = filtered_objects.objects.at(j);
if (node_param_.enable_z_axis_obstacle_filtering) {
if (!intersectsInZAxis(obj, z_axis_min, z_axis_max)) {
continue;
}
}
Polygon2d one_step_move_collision_polygon;
bool found_collision_object = false;
Polygon2d object_polygon{};
if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) {
object_polygon = convertCylindricalObjectToGeometryPolygon(
obj.kinematics.initial_pose_with_covariance.pose, obj.shape);

createOneStepPolygon(
p_front, p_back, one_step_move_collision_polygon, vehicle_info,
stop_param.pedestrian_lateral_margin);

found_collision_object = bg::intersects(one_step_move_collision_polygon, object_polygon);
} else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
const double & length_m = obj.shape.dimensions.x / 2;
const double & width_m = obj.shape.dimensions.y / 2;
object_polygon = convertBoundingBoxObjectToGeometryPolygon(
obj.kinematics.initial_pose_with_covariance.pose, length_m, length_m, width_m);
createOneStepPolygon(
p_front, p_back, one_step_move_collision_polygon, vehicle_info,
stop_param.vehicle_lateral_margin);

found_collision_object = bg::intersects(one_step_move_collision_polygon, object_polygon);
} else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) {
object_polygon = convertPolygonObjectToGeometryPolygon(
obj.kinematics.initial_pose_with_covariance.pose, obj.shape);
createOneStepPolygon(
p_front, p_back, one_step_move_collision_polygon, vehicle_info,
stop_param.unknown_lateral_margin);

found_collision_object = bg::intersects(one_step_move_collision_polygon, object_polygon);
} else {
RCLCPP_WARN_THROTTLE(
get_logger(), *get_clock(), 3000, "Object type is not supported. type: %d",
obj.shape.type);
continue;
}
if (found_collision_object) {
geometry_msgs::msg::PoseArray collision_points_tmp;

std::vector<Point2d> collision_point;
bg::intersection(one_step_move_collision_polygon, object_polygon, collision_point);
for (const auto & point : collision_point) {
geometry_msgs::msg::Pose pose;
pose.position.x = point.x();
pose.position.y = point.y();
collision_points_tmp.poses.push_back(pose);
}

// Also check the corner points
for (const auto & point : object_polygon.outer()) {
if (bg::within(point, one_step_move_collision_polygon)) {
geometry_msgs::msg::Pose pose;
pose.position.x = point.x();
pose.position.y = point.y();
collision_points_tmp.poses.push_back(pose);
}
}
geometry_msgs::msg::Point nearest_collision_point_tmp;
const double norm = getNearestPointAndDistanceForPredictedObject(
collision_points_tmp, p_front, &nearest_collision_point_tmp);
if (norm < min_collision_norm || !is_init) {
min_collision_norm = norm;
nearest_collision_point = nearest_collision_point_tmp;
is_init = true;
nearest_collision_object_index = j;
}
addPredictedObstacleToHistory(obj, now);
collision_found_on_trajectory_point = true;
}
}
if (is_init) {
predicted_object_history_.emplace_back(
now, nearest_collision_point,
filtered_objects.objects.at(nearest_collision_object_index));
break;
}

// only used for pedestrian
Polygon2d one_step_move_collision_dbg;
createOneStepPolygon(
p_front, p_back, one_step_move_collision_dbg, vehicle_info,
stop_param.pedestrian_lateral_margin);
if (node_param_.enable_z_axis_obstacle_filtering) {
debug_ptr_->pushPolyhedron(
one_step_move_collision_dbg, z_axis_min, z_axis_max, PolygonType::Vehicle);
} else {
debug_ptr_->pushPolygon(
one_step_move_collision_dbg, p_front.position.z, PolygonType::Vehicle);
}

if (collision_found_on_trajectory_point) {
break;
}
}
}

for (size_t i = 0; i < decimate_trajectory.size() - 1; ++i) {
if (predicted_object_history_.empty()) {
break;
}

// create one step circle center for vehicle
const auto & p_front = decimate_trajectory.at(i).pose;
const auto & p_back = decimate_trajectory.at(i + 1).pose;
const auto z_axis_min = p_front.position.z;
const auto z_axis_max =
p_front.position.z + vehicle_info.vehicle_height_m + node_param_.z_axis_filtering_buffer;

double min_collision_norm = 0.0;
bool is_init = false;
size_t nearest_collision_object_index = 0;
std::vector<IntersectedPredictedObject> intersected_predicted_obj{};

for (size_t j = 0; j < predicted_object_history_.size(); ++j) {
// check new collision points
const auto & obj = predicted_object_history_.at(j).object;
if (node_param_.enable_z_axis_obstacle_filtering) {
if (!intersectsInZAxis(obj, z_axis_min, z_axis_max)) {
continue;
}
}
Point2d collision_point;
collision_point.x() = predicted_object_history_.at(j).point.x;
collision_point.y() = predicted_object_history_.at(j).point.y;
Polygon2d object_polygon{};
Polygon2d one_step_move_vehicle_polygon;
// create one step polygon for vehicle
bool found_collision_object = false;
if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) {
object_polygon = convertCylindricalObjectToGeometryPolygon(

Check warning on line 847 in planning/obstacle_stop_planner/src/node.cpp

View check run for this annotation

Codecov / codecov/patch

planning/obstacle_stop_planner/src/node.cpp#L847 

Added line #L847 was not covered by tests
obj.kinematics.initial_pose_with_covariance.pose, obj.shape);

createOneStepPolygon(
p_front, p_back, one_step_move_vehicle_polygon, vehicle_info,
stop_param.pedestrian_lateral_margin);

found_collision_object = bg::intersects(one_step_move_vehicle_polygon, object_polygon);
} else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
const double & length_m = obj.shape.dimensions.x / 2;
const double & width_m = obj.shape.dimensions.y / 2;
object_polygon = convertBoundingBoxObjectToGeometryPolygon(

Check warning on line 858 in planning/obstacle_stop_planner/src/node.cpp

View check run for this annotation

Codecov / codecov/patch

planning/obstacle_stop_planner/src/node.cpp#L856-L858 

Added lines #L856 - L858 were not covered by tests
obj.kinematics.initial_pose_with_covariance.pose, length_m, length_m, width_m);
createOneStepPolygon(
p_front, p_back, one_step_move_vehicle_polygon, vehicle_info,
stop_param.vehicle_lateral_margin);

found_collision_object = bg::intersects(one_step_move_vehicle_polygon, object_polygon);
} else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) {
object_polygon = convertPolygonObjectToGeometryPolygon(

Check warning on line 866 in planning/obstacle_stop_planner/src/node.cpp

View check run for this annotation

Codecov / codecov/patch

planning/obstacle_stop_planner/src/node.cpp#L866 

Added line #L866 was not covered by tests
obj.kinematics.initial_pose_with_covariance.pose, obj.shape);
createOneStepPolygon(
p_front, p_back, one_step_move_vehicle_polygon, vehicle_info,
stop_param.unknown_lateral_margin);

found_collision_object = bg::intersects(one_step_move_vehicle_polygon, object_polygon);
} else {
RCLCPP_WARN_THROTTLE(
get_logger(), *get_clock(), 3000, "Object type is not supported. type: %d",
obj.shape.type);
continue;
}
if (bg::within(collision_point, one_step_move_vehicle_polygon)) {
const double norm = calcDistance2d(predicted_object_history_.at(j).point, p_front.position);
if (found_collision_object) {
intersected_predicted_obj.emplace_back(

Check warning on line 880 in planning/obstacle_stop_planner/src/node.cpp

View check run for this annotation

Codecov / codecov/patch

planning/obstacle_stop_planner/src/node.cpp#L880 

Added line #L880 was not covered by tests
predicted_object_history_.at(j).detection_time, obj, object_polygon,
one_step_move_vehicle_polygon);
}
}

Check warning on line 884 in planning/obstacle_stop_planner/src/node.cpp

View check run for this annotation

Codecov / codecov/patch

planning/obstacle_stop_planner/src/node.cpp#L884 

Added line #L884 was not covered by tests

planner_data.found_collision_points = !intersected_predicted_obj.empty();

Check warning on line 886 in planning/obstacle_stop_planner/src/node.cpp

View check run for this annotation

Codecov / codecov/patch

planning/obstacle_stop_planner/src/node.cpp#L886 

Added line #L886 was not covered by tests

if (planner_data.found_collision_points) {
// find the nearest point for each object polygon
double min_collision_norm = 0.0;
bool is_init = false;
size_t nearest_collision_object_index = 0;
geometry_msgs::msg::Point nearest_collision_point;

for (size_t j = 0; j < intersected_predicted_obj.size(); ++j) {
const auto & one_step_move_vehicle_polygon =
intersected_predicted_obj.at(j).vehicle_polygon;

Check warning on line 897 in planning/obstacle_stop_planner/src/node.cpp

View check run for this annotation

Codecov / codecov/patch

planning/obstacle_stop_planner/src/node.cpp#L897 

Added line #L897 was not covered by tests
const auto & obj_polygon = intersected_predicted_obj.at(j).object_polygon;
geometry_msgs::msg::PoseArray collision_points_tmp;

std::vector<Point2d> collision_point;
bg::intersection(one_step_move_vehicle_polygon, obj_polygon, collision_point);
for (const auto & point : collision_point) {
geometry_msgs::msg::Pose pose;
pose.position.x = point.x();
pose.position.y = point.y();

Check warning on line 906 in planning/obstacle_stop_planner/src/node.cpp

View check run for this annotation

Codecov / codecov/patch

planning/obstacle_stop_planner/src/node.cpp#L905-L906 

Added lines #L905 - L906 were not covered by tests
collision_points_tmp.poses.push_back(pose);
}

// Also check the corner points
for (const auto & point : obj_polygon.outer()) {
if (bg::within(point, one_step_move_vehicle_polygon)) {
geometry_msgs::msg::Pose pose;
pose.position.x = point.x();
pose.position.y = point.y();

Check warning on line 915 in planning/obstacle_stop_planner/src/node.cpp

View check run for this annotation

Codecov / codecov/patch

planning/obstacle_stop_planner/src/node.cpp#L914-L915 

Added lines #L914 - L915 were not covered by tests
collision_points_tmp.poses.push_back(pose);
}
}
geometry_msgs::msg::Point nearest_collision_point_tmp;
const double norm = getNearestPointAndDistanceForPredictedObject(
collision_points_tmp, p_front, &nearest_collision_point_tmp);
if (norm < min_collision_norm || !is_init) {
min_collision_norm = norm;
nearest_collision_point = nearest_collision_point_tmp;

Check warning on line 924 in planning/obstacle_stop_planner/src/node.cpp

View check run for this annotation

Codecov / codecov/patch

planning/obstacle_stop_planner/src/node.cpp#L924 

Added line #L924 was not covered by tests
is_init = true;
nearest_collision_object_index = j;
}
}
}

planner_data.found_collision_points = is_init;

if (planner_data.found_collision_points) {
planner_data.nearest_collision_point = pointToPcl(
predicted_object_history_.at(nearest_collision_object_index).point.x,
predicted_object_history_.at(nearest_collision_object_index).point.y, p_front.position.z);
planner_data.nearest_collision_point =
pointToPcl(nearest_collision_point.x, nearest_collision_point.y, p_front.position.z);

planner_data.decimate_trajectory_collision_index = i;

planner_data.nearest_collision_point_time =
predicted_object_history_.at(nearest_collision_object_index).detection_time;
intersected_predicted_obj.at(nearest_collision_object_index).detection_time;

// create one step polygon for vehicle collision debug
Polygon2d one_step_move_vehicle_polygon;
Polygon2d object_polygon{};

const auto & obj = predicted_object_history_.at(nearest_collision_object_index).object;
if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::CYLINDER) {
createOneStepPolygon(
p_front, p_back, one_step_move_vehicle_polygon, vehicle_info,
stop_param.pedestrian_lateral_margin);
object_polygon = convertCylindricalObjectToGeometryPolygon(
obj.kinematics.initial_pose_with_covariance.pose, obj.shape);
} else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
createOneStepPolygon(
p_front, p_back, one_step_move_vehicle_polygon, vehicle_info,
stop_param.vehicle_lateral_margin);
const double & length_m = obj.shape.dimensions.x / 2;
const double & width_m = obj.shape.dimensions.y / 2;
object_polygon = convertBoundingBoxObjectToGeometryPolygon(
obj.kinematics.initial_pose_with_covariance.pose, length_m, length_m, width_m);

} else if (obj.shape.type == autoware_auto_perception_msgs::msg::Shape::POLYGON) {
createOneStepPolygon(
p_front, p_back, one_step_move_vehicle_polygon, vehicle_info,
stop_param.unknown_lateral_margin);
object_polygon = convertPolygonObjectToGeometryPolygon(
obj.kinematics.initial_pose_with_covariance.pose, obj.shape);
}
// debug
debug_ptr_->pushObstaclePoint(planner_data.nearest_collision_point, PointType::Stop);

if (node_param_.enable_z_axis_obstacle_filtering) {
debug_ptr_->pushPolyhedron(
one_step_move_vehicle_polygon, z_axis_min, z_axis_max, PolygonType::Collision);
intersected_predicted_obj.at(nearest_collision_object_index).vehicle_polygon, z_axis_min,
z_axis_max, PolygonType::Collision);
} else {
debug_ptr_->pushPolygon(
one_step_move_vehicle_polygon, p_front.position.z, PolygonType::Collision);
intersected_predicted_obj.at(nearest_collision_object_index).vehicle_polygon,
p_front.position.z, PolygonType::Collision);
}

if (node_param_.publish_obstacle_polygon) {
debug_ptr_->pushPolygon(object_polygon, p_front.position.z, PolygonType::Obstacle);
debug_ptr_->pushPolygon(

Check warning on line 952 in planning/obstacle_stop_planner/src/node.cpp

View check run for this annotation

Codecov / codecov/patch

planning/obstacle_stop_planner/src/node.cpp#L952 

Added line #L952 was not covered by tests
intersected_predicted_obj.at(nearest_collision_object_index).object_polygon,
p_front.position.z, PolygonType::Obstacle);
}

planner_data.stop_require = planner_data.found_collision_points;
Expand All @@ -970,10 +960,12 @@
const auto latest_object_ptr = object_ptr_;
mutex_.unlock();
// find latest state of predicted object to get latest velocity and acceleration values
auto obj_latest_state = getObstacleFromUuid(*latest_object_ptr, obj.object_id);
auto obj_latest_state = getObstacleFromUuid(
*latest_object_ptr,
intersected_predicted_obj.at(nearest_collision_object_index).object.object_id);
if (!obj_latest_state) {
// Can not find the object in the latest object list. Send previous state.
obj_latest_state = obj;
obj_latest_state = intersected_predicted_obj.at(nearest_collision_object_index).object;

Check notice on line 968 in planning/obstacle_stop_planner/src/node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Bumpy Road Ahead 

ObstacleStopPlannerNode::searchPredictedObject decreases from 11 to 9 logical blocks with deeply nested code, threshold is one single block per function. 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 notice on line 968 in planning/obstacle_stop_planner/src/node.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Complex Method 

ObstacleStopPlannerNode::searchPredictedObject decreases in cyclomatic complexity from 59 to 55, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.
}

acc_controller_->insertAdaptiveCruiseVelocity(
Expand Down
Loading