Skip to content

Commit

Permalink
not finished
Browse files Browse the repository at this point in the history
  • Loading branch information
@brkay54
brkay54 committed Dec 5, 2023
1 parent 60b4030 commit bba2911
Show file tree
Hide file tree
Showing 2 changed files with 111 additions and 84 deletions.
5 changes: 2 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,13 +108,12 @@ 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;
};

Expand Down
190 changes: 109 additions & 81 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<std::pair<Polygon2d, PredictedObject>> collision_polygons_and_obj_on_footprint{};

for (size_t j = 0; j < predicted_object_history_.size(); ++j) {
// check new collision points
Expand All @@ -875,45 +840,108 @@ 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 (norm < min_collision_norm || !is_init) {
min_collision_norm = norm;
is_init = true;
nearest_collision_object_index = j;
}
if (found_collision_object) {
collision_polygons_and_obj_on_footprint.emplace_back(object_polygon, obj);
}
}

planner_data.found_collision_points = is_init;
planner_data.found_collision_points = !collision_polygons_and_obj_on_footprint.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 < collision_polygons_and_obj_on_footprint.size(); ++j) {
Polygon2d one_step_move_vehicle_polygon;
const auto & obj = collision_polygons_and_obj_on_footprint.at(j).second;
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);
} 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);
} 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);
} else {
RCLCPP_WARN_THROTTLE(
get_logger(), *get_clock(), 3000, "Object type is not supported. type: %d",
obj.shape.type);
continue;
}
geometry_msgs::msg::PoseArray collision_points_tmp;

std::vector<Point2d> collision_point;
bg::intersection(one_step_move_vehicle_polygon, collision_polygons_and_obj_on_footprint.at(j).first, 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 : collision_polygons_and_obj_on_footprint.at(j).first.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.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);
Expand Down Expand Up @@ -1060,16 +1088,16 @@ void ObstacleStopPlannerNode::insertVelocity(
const auto ego_pos_on_path = calcLongitudinalOffsetPose(output, ego_pos, 0.0);

if (ego_pos_on_path) {
StopPoint current_stop_pos{};
current_stop_pos.index = findFirstNearestSegmentIndexWithSoftConstraints(
output, ego_pose, node_param_.ego_nearest_dist_threshold,
node_param_.ego_nearest_yaw_threshold);
current_stop_pos.point.pose = ego_pos_on_path.value();
StopPoint current_stop_pos{};
current_stop_pos.index = findFirstNearestSegmentIndexWithSoftConstraints(
output, ego_pose, node_param_.ego_nearest_dist_threshold,
node_param_.ego_nearest_yaw_threshold);
current_stop_pos.point.pose = ego_pos_on_path.value();

insertStopPoint(current_stop_pos, output, planner_data.stop_reason_diag);
insertStopPoint(current_stop_pos, output, planner_data.stop_reason_diag);

debug_ptr_->pushPose(getPose(stop_point.point), PoseType::TargetStop);
debug_ptr_->pushPose(getPose(current_stop_pos.point), PoseType::Stop);
debug_ptr_->pushPose(getPose(stop_point.point), PoseType::TargetStop);
debug_ptr_->pushPose(getPose(current_stop_pos.point), PoseType::Stop);
}

} else {
Expand Down Expand Up @@ -1134,31 +1162,31 @@ void ObstacleStopPlannerNode::insertVelocity(
if (
planner_data.slow_down_object_shape.type ==
autoware_auto_perception_msgs::msg::Shape::CYLINDER) {
slow_down_velocity =
slow_down_param_.min_slow_down_velocity +
(slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) *
std::max(planner_data.lateral_deviation - vehicle_info.vehicle_width_m / 2, 0.0) /
slow_down_param_.pedestrian_lateral_margin;
slow_down_velocity =
slow_down_param_.min_slow_down_velocity +
(slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) *
std::max(planner_data.lateral_deviation - vehicle_info.vehicle_width_m / 2, 0.0) /
slow_down_param_.pedestrian_lateral_margin;
} else if (
planner_data.slow_down_object_shape.type ==
autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
slow_down_velocity =
slow_down_param_.min_slow_down_velocity +
(slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) *
std::max(planner_data.lateral_deviation - vehicle_info.vehicle_width_m / 2, 0.0) /
slow_down_param_.vehicle_lateral_margin;
slow_down_velocity =
slow_down_param_.min_slow_down_velocity +
(slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) *
std::max(planner_data.lateral_deviation - vehicle_info.vehicle_width_m / 2, 0.0) /
slow_down_param_.vehicle_lateral_margin;
} else if (
planner_data.slow_down_object_shape.type ==
autoware_auto_perception_msgs::msg::Shape::POLYGON) {
slow_down_velocity =
slow_down_param_.min_slow_down_velocity +
(slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) *
std::max(planner_data.lateral_deviation - vehicle_info.vehicle_width_m / 2, 0.0) /
slow_down_param_.unknown_lateral_margin;
slow_down_velocity =
slow_down_param_.min_slow_down_velocity +
(slow_down_param_.max_slow_down_velocity - slow_down_param_.min_slow_down_velocity) *
std::max(planner_data.lateral_deviation - vehicle_info.vehicle_width_m / 2, 0.0) /
slow_down_param_.unknown_lateral_margin;
} else {
RCLCPP_WARN_THROTTLE(
get_logger(), *get_clock(), 3000, "Object type is not supported. type: %d",
planner_data.slow_down_object_shape.type);
RCLCPP_WARN_THROTTLE(
get_logger(), *get_clock(), 3000, "Object type is not supported. type: %d",
planner_data.slow_down_object_shape.type);
}
} else {
slow_down_velocity =
Expand Down

0 comments on commit bba2911

Please sign in to comment.