Skip to content

Commit

Permalink
fix(obstacle_stop_planner): change the nearest_collision_point calcul…
Browse files Browse the repository at this point in the history 
…ation place

Signed-off-by: Berkay Karaman <[email protected]>
  • Loading branch information
@brkay54
brkay54 committed Dec 6, 2023
1 parent 60b4030 commit e384998
Show file tree
Hide file tree
Showing 2 changed files with 97 additions and 92 deletions.
19 changes: 16 additions & 3 deletions planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -108,16 +108,29 @@ struct ObstacleWithDetectionTime
struct PredictedObjectWithDetectionTime
{
explicit PredictedObjectWithDetectionTime(
const rclcpp::Time & t, geometry_msgs::msg::Point & p, PredictedObject obj)
: detection_time(t), point(p), object(std::move(obj))
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(
const rclcpp::Time & t, PredictedObject obj, const Polygon2d obj_polygon, const Polygon2d ego_polygon)
: detection_time(t), object(std::move(obj)), object_polygon{obj_polygon}, vehicle_polygon{ego_polygon}
{
}

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

class ObstacleStopPlannerNode : public rclcpp::Node
{
public:
Expand Down
170 changes: 81 additions & 89 deletions planning/obstacle_stop_planner/src/node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -749,11 +749,7 @@ void ObstacleStopPlannerNode::searchPredictedObject(
}

{
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) {
Expand Down Expand Up @@ -798,43 +794,10 @@ void ObstacleStopPlannerNode::searchPredictedObject(
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;
}
predicted_object_history_.emplace_back(now, obj);
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;
Expand All @@ -848,6 +811,10 @@ void ObstacleStopPlannerNode::searchPredictedObject(
debug_ptr_->pushPolygon(
one_step_move_collision_dbg, p_front.position.z, PolygonType::Vehicle);
}

if (collision_found_on_trajectory_point) {
break;
}
}
}

Expand All @@ -863,9 +830,7 @@ void ObstacleStopPlannerNode::searchPredictedObject(
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
Expand All @@ -875,93 +840,118 @@ void ObstacleStopPlannerNode::searchPredictedObject(
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(
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(
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(
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(
predicted_object_history_.at(j).detection_time, obj, object_polygon,
one_step_move_vehicle_polygon);
}
}

planner_data.found_collision_points = !intersected_predicted_obj.empty();

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;
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();
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();
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;
}
}
}

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(
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 @@ void ObstacleStopPlannerNode::searchPredictedObject(
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

0 comments on commit e384998

Please sign in to comment.