diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp index cc1ad6999b898..5bf5dbe80bab5 100644 --- a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp +++ b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp @@ -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; }; diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp index 050c4af55c9bf..4013b4abe2bdc 100644 --- a/planning/obstacle_stop_planner/src/node.cpp +++ b/planning/obstacle_stop_planner/src/node.cpp @@ -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) { @@ -798,43 +794,10 @@ void ObstacleStopPlannerNode::searchPredictedObject( continue; } if (found_collision_object) { - geometry_msgs::msg::PoseArray collision_points_tmp; - - std::vector 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; @@ -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; + } } } @@ -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> collision_polygons_and_obj_on_footprint{}; for (size_t j = 0; j < predicted_object_history_.size(); ++j) { // check new collision points @@ -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 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); @@ -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 { @@ -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 =