void AiDetection::calculateBrightness(cv::Mat &_image, int &brightnessLevel) {
auto start_b = std::chrono::high_resolution_clock::now();
// 获取图像高度并计算上半部分的高度
// int height = _image.size().height;
// int halfHeight = height / 2;
// // 提取图像上半部分
// cv::Mat upperHalfImage = _image(cv::Range(0, halfHeight), cv::Range::all());
cv::Mat grayscaleImage;
cv::cvtColor(_image, grayscaleImage, cv::COLOR_BGR2GRAY); // 转为灰度图
// 计算灰度图像的平均亮度
cv::Scalar meanBrightness = cv::mean(grayscaleImage);
int brightness = static_cast<int>(meanBrightness.val[0]);
// cv::Scalar median;
// cv::medianBlur(grayscaleImage, grayscaleImage, 1);
// cv::minMaxLoc(grayscaleImage, nullptr, &median.val[0], nullptr, nullptr, cv::Mat());
// int brightness = static_cast<int>(median.val[0]);
if (brightness < 40) {
LOG_TRACE("Brightness Level: very dark");
brightnessLevel = nav2_msgs::action::Brightness::VERY_DARK;
} else if (brightness < 75) {
LOG_TRACE("Brightness Level: dark");
brightnessLevel = nav2_msgs::action::Brightness::GENERALLY_DARK;
} else if (brightness < 140) {
LOG_TRACE("Brightness Level: normal");
brightnessLevel = nav2_msgs::action::Brightness::GENERALLY_BRIGHT;
} else {
LOG_TRACE("Brightness Level: light");
brightnessLevel = nav2_msgs::action::Brightness::VERY_BRIGHT;
}
auto end_b = std::chrono::high_resolution_clock::now();
std::chrono::duration<double> diff_b = end_b - start_b;
LOG_TRACE("*************brightness detect time: {} *************", diff_b.count());
}
void AiDetection::optimizedBrightnessCalculation(cv::Mat &_image, int &brightnessLevel) {
auto start_b = std::chrono::high_resolution_clock::now();
// 定义检测区域,左上角 (360, 0),右下角 (900, 400)
cv::Rect roi(360, 0, 900 - 360, 400 - 0);
cv::Mat targetArea = _image(roi);
// 图像预处理:去噪
cv::Mat denoisedImage;
cv::medianBlur(targetArea, denoisedImage, 5); // 使用5x5的中值滤波器去噪
// 转换为灰度图像
cv::Mat grayscaleImage;
cv::cvtColor(denoisedImage, grayscaleImage, cv::COLOR_BGR2GRAY);
// 计算灰度图像的平均亮度
cv::Scalar meanBrightness = cv::mean(grayscaleImage);
int brightness = static_cast<int>(meanBrightness.val[0]);
// 根据亮度值确定亮度级别
if (brightness < 40) {
LOG_TRACE("Brightness Level: very dark");
brightnessLevel = nav2_msgs::action::Brightness::VERY_DARK;
} else if (brightness < 95) {
LOG_TRACE("Brightness Level: dark");
brightnessLevel = nav2_msgs::action::Brightness::GENERALLY_DARK;
} else if (brightness < 140) {
LOG_TRACE("Brightness Level: normal");
brightnessLevel = nav2_msgs::action::Brightness::GENERALLY_BRIGHT;
} else {
LOG_TRACE("Brightness Level: light");
brightnessLevel = nav2_msgs::action::Brightness::VERY_BRIGHT;
}
auto end_b = std::chrono::high_resolution_clock::now();
std::chrono::duration<double> diff_b = end_b - start_b;
LOG_TRACE("*************brightness level count: {} *************", brightness);
LOG_TRACE("*************brightness detect time: {} *************", diff_b.count());
}void AiServer::ai_detect()
{
enum State {
NORMAL_STATE,
DARK_CHECK_STATE,
SUSTAIN_PUBLISH_STATE,
POST_SUSTAIN_CHECK_STATE
};
int check_mem_count = 0;
State current_state = NORMAL_STATE;
int dark_check_counter = 0;
int brightness_dark_check_counts[2] = {0};
int sustain_publish_counter = 0;
int sustain_publish_interval_counter = 0;
int post_sustain_check_counter = 0;
int post_sustain_brightness_counts[2] = {0};
try {
std::string raw_path;
cv::Mat frame;
int queue_size = 0;
TOF_Frame tof_frame;
cv::Mat R = cv::Mat::eye(3, 3, CV_64F);
cv::Size img_size(rgb_tof_dev_w_h_tmp.rgb_w, rgb_tof_dev_w_h_tmp.rgb_h);
cv::Mat mapx, mapy;
cv::fisheye::initUndistortRectifyMap(camera_matrix, distortion_coefficients, R, camera_matrix, img_size, CV_32FC1, mapx,
mapy);
cv::Mat resultImg;
nav2_msgs::action::AiDetection_Result brightness_level;
int currentBrightnessLevel;
int brightness_calculation_count = 0;
int brightness_low_streak = 0;
int brightness_high_streak = 0;
double loop_frequency_ = 10;
rclcpp::WallRate r(loop_frequency_);
rclcpp::Rate a(1);
cv::Mat show_image;
sensor_msgs::msg::Image show_Image;
sensor_msgs::msg::PointCloud pointcloud;
bool use_dark_method = false;
bool use_pointcloud = true;
int dark_result_publish_count = 0;
int get_frame_error = 0;
int dark_result_count = 0;
int result_count = 0;
robot_msgs::msg::ErrorInfo error_info;
int block_counter = 0;
int unblock_counter = 0;
const int BLOCK_THRESHOLD = 20; // 2秒
const int UNBLOCK_THRESHOLD = 20;
bool isblocked = false;
cv::Rect seg_roi(30,208,1024,512);
while (true)
{
if(debug_){
check_mem_count++;
if(check_mem_count > 100){
check_mem_count = 0;
trace_mem();
}
}
if(is_ai_detection_){
LOG_TRACE("===================================start=========================================");
auto start = std::chrono::high_resolution_clock::now();
if(camera_base_->getFrame(raw_path, frame,tof_frame, queue_size))
{
get_frame_error = 0;
frame_error_ = false;
auto end_getFrame = std::chrono::high_resolution_clock::now();
std::chrono::duration<double> diff_get_frame = end_getFrame - start;
LOG_TRACE("camera_base_->getFrame size: {}, time: {} ",queue_size, diff_get_frame.count());
{
auto start_dection = std::chrono::high_resolution_clock::now();
if(is_edge_detection_){
ai_detection_->edge_detection(frame, *tof_frame.pRgb2TofPixelCoord, tof_frame.pointcloud);
}
else if (is_seg_edge_detection_){
ai_detection_->seg_edge_detection(frame, *tof_frame.pRgb2TofPixelCoord, tof_frame.pointcloud);
}
else{
ai_detection_->detection(frame, *tof_frame.pRgb2TofPixelCoord, tof_frame.pointcloud, avoidence_size, sensitivity_level);
}
LOG_TRACE("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!sensitivity_level: {} !!!!!!!!!!!!!!!!!!!!!!!!!!!!!1:detect_only,2:no_believe_seg,3:yes_believe_seg", sensitivity_level);
LOG_TRACE("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!avoidence_size: {} !!!!!!!!!!!!!!!!!!!!!!!!!!!!!", avoidence_size);
auto end_dection = std::chrono::high_resolution_clock::now();
std::chrono::duration<double> diff_dection = end_dection - start_dection;
LOG_TRACE("*************detection Time to run:{} seconds.*************",diff_dection.count());
}
// if (result_count % 5 == 0){
// if(is_seg_edge_detection_){
// cv::Rect build_roi(0,208,1024,512);
// show_image = ai_detection_->getResultImage(frame, *tof_frame.pRgb2TofPixelCoord, build_roi);
// cv::Mat pub_image;
// int resize_W = 240;
// int resize_H = 136;
// cv::resize(show_image, pub_image, cv::Size(resize_W, resize_H));
// std::string dir_path = "/home/sunrise/result";
// std::string filename = dir_path + "/result_" + std::to_string(result_count) + ".jpg";
// cv::imwrite(filename, pub_image);
// }
// else{
// show_image = ai_detection_->getResultImage(frame, *tof_frame.pRgb2TofPixelCoord, seg_roi);
// cv::Mat pub_image;
// int resize_W = 240;
// int resize_H = 136;
// cv::resize(show_image, pub_image, cv::Size(resize_W, resize_H));
// std::string dir_path = "/home/sunrise/result";
// std::string filename = dir_path + "/result_" + std::to_string(result_count) + ".jpg";
// cv::imwrite(filename, pub_image);
// }
// }
result_count++;
brightness_calculation_count++;
if (brightness_calculation_count >= 5) {
if (use_dark_method) {
ai_detection_->calculateDarkBrightness(frame, currentBrightnessLevel);
} else {
ai_detection_->optimizedBrightnessCalculation(frame, currentBrightnessLevel);
}
brightness_calculation_count = 0;
// 状态机处理
switch(current_state) {
case DARK_CHECK_STATE:
if(++dark_check_counter == 1) { // 首次进入时记录
LOG_TRACE("Enter DARK_CHECK_STATE, start 30 brightness checks");
}
if(currentBrightnessLevel <= 1) brightness_dark_check_counts[0]++;
else brightness_dark_check_counts[1]++;
if(++dark_check_counter >= 30) {
if(brightness_dark_check_counts[0] > brightness_dark_check_counts[1]) {
publishBrightnesslevel(0, brightness_level);
current_state = NORMAL_STATE;
use_dark_method = false;
LOG_TRACE("Exit DARK_CHECK_STATE: Bright dominant");
} else {
current_state = SUSTAIN_PUBLISH_STATE;
sustain_publish_counter = 0;
sustain_publish_interval_counter = 0;
LOG_TRACE("Exit DARK_CHECK_STATE: Dark dominant");
}
memset(brightness_dark_check_counts, 0, sizeof(brightness_dark_check_counts));
dark_check_counter = 0;
}
break;
case POST_SUSTAIN_CHECK_STATE:
if(post_sustain_check_counter == 0) { // 首次进入时记录
LOG_TRACE("Enter POST_SUSTAIN_CHECK_STATE, start 10 checks");
}
if(currentBrightnessLevel <= 1) post_sustain_brightness_counts[0]++;
else post_sustain_brightness_counts[1]++;
if(++post_sustain_check_counter >= 10) {
LOG_TRACE("PostCheck results - Bright:{} Dark:{}",
post_sustain_brightness_counts[0],
post_sustain_brightness_counts[1]);
if(post_sustain_brightness_counts[0] > post_sustain_brightness_counts[1]) {
publishBrightnesslevel(0, brightness_level);
current_state = NORMAL_STATE;
use_dark_method = false;
LOG_TRACE("Return to NORMAL_STATE from POST_CHECK");
} else {
current_state = SUSTAIN_PUBLISH_STATE;
sustain_publish_counter = 0;
sustain_publish_interval_counter = 0;
LOG_TRACE("Re-enter SUSTAIN_PUBLISH_STATE");
}
memset(post_sustain_brightness_counts, 0, sizeof(post_sustain_brightness_counts));
post_sustain_check_counter = 0;
}
break;
}
// 原始区间判断逻辑保持不变
if (currentBrightnessLevel == 0 || currentBrightnessLevel == 1) {
brightness_high_streak++;
brightness_low_streak = 0;
} else if (currentBrightnessLevel == 2 || currentBrightnessLevel == 3) {
brightness_low_streak++;
brightness_high_streak = 0;
} else {
brightness_high_streak = 0;
brightness_low_streak = 0;
}
}
// 状态触发逻辑
if ((brightness_high_streak >= 12 || brightness_low_streak >= 10) && current_state == NORMAL_STATE) {
publishBrightnesslevel(currentBrightnessLevel, brightness_level);
LOG_TRACE("Publish Brightness_level:{}", currentBrightnessLevel);
brightness_high_streak = 0;
brightness_low_streak = 0;
use_dark_method = (currentBrightnessLevel >= 2);
if(use_dark_method) {
current_state = DARK_CHECK_STATE;
dark_check_counter = 0;
memset(brightness_dark_check_counts, 0, sizeof(brightness_dark_check_counts));
}
}
// 持续发布状态处理
if(current_state == SUSTAIN_PUBLISH_STATE) {
if(++sustain_publish_counter >= 600) { // 60秒
current_state = POST_SUSTAIN_CHECK_STATE;
post_sustain_check_counter = 0;
memset(post_sustain_brightness_counts, 0, sizeof(post_sustain_brightness_counts));
} else if(++sustain_publish_interval_counter >= 100) { // 10秒
publishBrightnesslevel(2, brightness_level);
LOG_TRACE("Sustain Publish Brightness_level:2");
sustain_publish_interval_counter = 0;
}
}
ai_detection_->checkblock(frame, isblocked);
if(isblocked) {
unblock_counter = 0; // 重置解除计数器
if(!blocked_error_) {
if(++block_counter >= BLOCK_THRESHOLD) {
LOG_TRACE("Camera blocked for 2 seconds!");
blocked_error_ = true;
publishError(error_info);
}
}
} else {
block_counter = 0; // 重置触发计数器
if(blocked_error_) {
if(++unblock_counter >= UNBLOCK_THRESHOLD) {
LOG_TRACE("Camera unblocked for 2 seconds!");
blocked_error_ = false;
unblock_counter = 0;
}
}
}
// 保持原有图像发布逻辑
if(debug_){
if (show_image_pub_->get_subscription_count() > 0) {
if(is_seg_edge_detection_){
cv::Rect build_roi(30,208,1024,512);
show_image = ai_detection_->getResultImage(frame, *tof_frame.pRgb2TofPixelCoord, build_roi);
cv::Mat pub_image;
int resize_W = 240;
int resize_H = 136;
cv::resize(show_image, pub_image, cv::Size(resize_W, resize_H));
cv::cvtColor(pub_image, pub_image, cv::COLOR_BGR2RGB);
publishImage(show_image_pub_, pub_image);
}
else{
show_image = ai_detection_->getResultImage(frame, *tof_frame.pRgb2TofPixelCoord, seg_roi);
cv::Mat pub_image;
int resize_W = 240;
int resize_H = 136;
cv::resize(show_image, pub_image, cv::Size(resize_W, resize_H));
cv::cvtColor(pub_image, pub_image, cv::COLOR_BGR2RGB);
publishImage(show_image_pub_, pub_image);
}
}
}
// 保持原有点云发布逻辑
if (is_seg_edge_detection_){
if (show_seg_pointcloud_pub_->get_subscription_count() > 0) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_result;
ai_detection_->getResultPointclound(cloud_result);
sensor_msgs::msg::PointCloud ros_pointcloud;
conversion_pcl_to_ros(cloud_result, ros_pointcloud);
rclcpp::Time t(tof_frame.host_ts, RCL_SYSTEM_TIME);
ros_pointcloud.header.stamp = t;
publishPointCloud(show_seg_pointcloud_pub_, ros_pointcloud);
}
}
if (show_det_pointcloud_pub_->get_subscription_count() > 0) {
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_result;
ai_detection_->getResultPointclound(cloud_result);
sensor_msgs::msg::PointCloud ros_pointcloud;
conversion_pcl_to_ros(cloud_result, ros_pointcloud);
rclcpp::Time t(tof_frame.host_ts, RCL_SYSTEM_TIME);
ros_pointcloud.header.stamp = t;
publishPointCloud(show_det_pointcloud_pub_, ros_pointcloud);
}
auto end = std::chrono::high_resolution_clock::now();
std::chrono::duration<double> diff = end - start;
LOG_TRACE("*************all detection Time to run:{} seconds.*************",diff.count());
if (!r.sleep()) {
RCLCPP_WARN(logger_, "ai_detect loop missed its desired rate of %.4fHz \n", loop_frequency_);
}
LOG_TRACE("================================end============================================");
}
else
{
LOG_WARN("getFrame time out!");
get_frame_error++;
if (get_frame_error >= 10){
frame_error_ = true;
}
}
}
else{
a.sleep();
}
}
} catch (const std::exception &e) {
RCLCPP_ERROR(logger_, "%s", e.what());
LOG_ERROR("*************all detection run exception:{} *************", e.what());
return;
}
}这是现在暂时使用的环境光检测流程和两种环境光计算方法,进行完整修改优化或者替换,方便能够快速使用编译测试