Rotate Image(二位数组顺时针旋转)

本文介绍了一个算法,用于将NxN的整数矩阵顺时针旋转90度。通过详细步骤解析和示例图,展示了如何在原地进行操作,避免额外的空间开销。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

问题描述:
You are given an n x n 2D matrix representing an image.
Rotate the image by 90 degrees (clockwise).
Follow up:
Could you do this in-place?
中文题目:
有一个NxN整数矩阵,请编写一个算法,将矩阵顺时针旋转90度。
给定一个NxN的矩阵,和矩阵的阶数N,请返回旋转后的NxN矩阵,保证N小于等于300。
分析:
过程如下图所示:

step1:

区域1和2内元素互换
step1的结果为:
step1的结果

step2:

区域1和4内元素互换
step2的结果为:
step2的结果

step3:

区域3和4内元素互换
step3的结果

step4:

上图的圆圈中的元素按照上述顺序互换

这样,一层结束了;

step5:

在二维数组的内层继续执行step1–step4,直到最内层结束;

代码如下:

class Rotate {
public:
    void swap(int &a,int &b)
{
    int temp = a;
    a = b;
    b = temp;
}
    vector<vector<int> > rotateMatrix(vector<vector<int> > mat, int n) {
    if(n == 1 || n == 0)
    return mat;

    int up  = 0;
    int right = n-1;
    int down = n-1;
    int left = 0;

    while(up < down && left < right)
    {
        //1,2 swap
        int begin1 = up + 1;
        for(int i = left + 1;i < right;i++)
        {
            swap(mat[up][i],mat[begin1++][right]);
        }
        //1 4 swap
        begin1 = down - 1;
        for(int i = left + 1;i < right;i++)
        {
            swap(mat[up][i],mat[begin1--][left]);
        }
        //3 4 swap
        begin1 = up + 1;
        for(int i = left + 1;i < right;i++)
        {
            swap(mat[down][i],mat[begin1++][left]);
        }
        //圆圈内的四个角上的元素互换
        swap(mat[up][left],mat[up][right]);

        swap(mat[up][left],mat[down][left]);

        swap(mat[down][left],mat[down][right]);

        up++;
        down--;
        left++;
        right--;
    }
    return mat; 
    }
};

算法实现在数组本地实现,空间复杂度为O(1).

<script type="text/javascript"> $(function () { $('pre.prettyprint code').each(function () { var lines = $(this).text().split('\n').length; var $numbering = $('<ul/>').addClass('pre-numbering').hide(); $(this).addClass('has-numbering').parent().append($numbering); for (i = 1; i <= lines; i++) { $numbering.append($('<li/>').text(i)); }; $numbering.fadeIn(1700); }); }); </script>
#include "ucar/ucar.hpp" #include <tf2/LinearMath/Quaternion.h> #include <ros/console.h> FileTransfer::FileTransfer(const std::string& baseDir ) : m_baseDir(baseDir), m_running(false), m_serverThread() { createDirectory(m_baseDir); } FileTransfer::~FileTransfer() { stopServer(); } // 功能1:保存字符串到本地文件 bool FileTransfer::saveToFile(const std::string& content, const std::string& filename) { std::string fullPath = m_baseDir + "/" + filename; std::ofstream file(fullPath); if (!file.is_open()) { std::cerr << "无法打开文件: " << fullPath << std::endl; return false; } file << content; file.close(); std::cout << "文件已保存到: " << fullPath << std::endl; return true; } // 功能2:发送文件到指定IP bool FileTransfer::sendTo(const std::string& serverIP, int port, const std::string& localFile, const std::string& remotePath ) { // 1. 读取文件内容 std::ifstream file(localFile, std::ios::binary | std::ios::ate); if (!file.is_open()) { std::cerr << "无法打开本地文件: " << localFile << std::endl; return false; } size_t fileSize = file.tellg(); file.seekg(0, std::ios::beg); std::vector<char> buffer(fileSize); if (!file.read(buffer.data(), fileSize)) { std::cerr << "读取文件错误: " << localFile << std::endl; return false; } file.close(); // 2. 创建Socket int sock = socket(AF_INET, SOCK_STREAM, 0); if (sock < 0) { std::cerr << "创建Socket失败: " << strerror(errno) << std::endl; return false; } // 3. 连接服务器 sockaddr_in serverAddr; memset(&serverAddr, 0, sizeof(serverAddr)); serverAddr.sin_family = AF_INET; serverAddr.sin_port = htons(port); if (inet_pton(AF_INET, serverIP.c_str(), &serverAddr.sin_addr) <= 0) { std::cerr << "无效的IP地址: " << serverIP << std::endl; close(sock); return false; } if (connect(sock, (sockaddr*)&serverAddr, sizeof(serverAddr)) < 0) { std::cerr << "连接服务器失败: " << strerror(errno) << std::endl; close(sock); return false; } // 4. 发送文件信息 if (!sendData(sock, remotePath, buffer.data(), fileSize)) { close(sock); return false; } close(sock); std::cout << "文件已发送到 " << serverIP << ":" << port << " 保存为 " << remotePath << std::endl; return true; } // 功能3:阻塞等待接收文件并读取内容 std::string FileTransfer::receiveAndRead(int port, int timeoutMs ) { // 确保服务器正在运行 if (!m_running) { startServer(port); } // 等待文件到达 ReceivedFile file; if (!waitForFile(file, timeoutMs)) { throw std::runtime_error("等待文件超时或服务器停止"); } stopServer(); // 返回文件内容 return std::string(file.content.data(), file.content.size()); } // 启动服务器(后台线程) void FileTransfer::startServer(int port) { if (m_running) { stopServer(); } m_running = true; m_serverThread = std::thread(&FileTransfer::serverThreadFunc, this, port); } // 停止服务器 void FileTransfer::stopServer() { if (m_running) { m_running = false; if (m_serverThread.joinable()) { m_serverThread.join(); } } } // 创建目录 bool FileTransfer::createDirectory(const std::string& path) { if (mkdir(path.c_str(), 0777) == -1 && errno != EEXIST) { std::cerr << "无法创建目录: " << path << " - " << strerror(errno) << std::endl; return false; } return true; } // 发送数据到socket bool FileTransfer::sendData(int sock, const std::string& remotePath, const char* data, size_t size) { // 1. 发送路径长度和路径 uint32_t pathLen = htonl(static_cast<uint32_t>(remotePath.size())); if (send(sock, &pathLen, sizeof(pathLen), 0) != sizeof(pathLen)) { std::cerr << "发送路径长度失败" << std::endl; return false; } if (send(sock, remotePath.c_str(), remotePath.size(), 0) != static_cast<ssize_t>(remotePath.size())) { std::cerr << "发送路径失败" << std::endl; return false; } // 2. 发送文件长度和内容 uint32_t netSize = htonl(static_cast<uint32_t>(size)); if (send(sock, &netSize, sizeof(netSize), 0) != sizeof(netSize)) { std::cerr << "发送文件长度失败" << std::endl; return false; } ssize_t totalSent = 0; while (totalSent < static_cast<ssize_t>(size)) { ssize_t sent = send(sock, data + totalSent, size - totalSent, 0); if (sent < 0) { std::cerr << "发送文件内容失败: " << strerror(errno) << std::endl; return false; } totalSent += sent; } return true; } // 服务器线程函数 void FileTransfer::serverThreadFunc(int port) { // 1. 创建Socket int listenSock = socket(AF_INET, SOCK_STREAM, 0); if (listenSock < 0) { std::cerr << "创建Socket失败: " << strerror(errno) << std::endl; return; } // 2. 设置SO_REUSEADDR int opt = 1; setsockopt(listenSock, SOL_SOCKET, SO_REUSEADDR, &opt, sizeof(opt)); // 3. 绑定端口 sockaddr_in serverAddr; memset(&serverAddr, 0, sizeof(serverAddr)); serverAddr.sin_family = AF_INET; serverAddr.sin_addr.s_addr = INADDR_ANY; serverAddr.sin_port = htons(port); if (bind(listenSock, (sockaddr*)&serverAddr, sizeof(serverAddr)) < 0) { std::cerr << "绑定端口失败: " << strerror(errno) << std::endl; close(listenSock); return; } // 4. 开始监听 if (listen(listenSock, 5) < 0) { std::cerr << "监听失败: " << strerror(errno) << std::endl; close(listenSock); return; } std::cout << "文件接收服务器启动,监听端口: " << port << std::endl; // 5. 接受连接循环 bool receivedFile = false; while (m_running&& !receivedFile) { // 设置超时以定期检查停止条件 fd_set readSet; FD_ZERO(&readSet); FD_SET(listenSock, &readSet); timeval timeout{0, 100000}; // 100ms int ready = select(listenSock + 1, &readSet, nullptr, nullptr, &timeout); if (ready < 0) { if (errno != EINTR) { std::cerr << "select错误: " << strerror(errno) << std::endl; } continue; } if (ready == 0) continue; // 超时,继续循环 // 6. 接受客户端连接 sockaddr_in clientAddr; socklen_t clientLen = sizeof(clientAddr); int clientSock = accept(listenSock, (sockaddr*)&clientAddr, &clientLen); if (clientSock < 0) { if (errno != EAGAIN && errno != EWOULDBLOCK) { std::cerr << "接受连接失败: " << strerror(errno) << std::endl; } continue; } char clientIP[INET_ADDRSTRLEN]; inet_ntop(AF_INET, &clientAddr.sin_addr, clientIP, INET_ADDRSTRLEN); std::cout << "收到来自 " << clientIP << " 的文件传输请求" << std::endl; // 7. 接收文件 ReceivedFile file; if (receiveFile(clientSock, file)) { std::lock_guard<std::mutex> lock(m_mutex); m_receivedFiles.push(std::move(file)); receivedFile = true; m_cv.notify_one(); // 通知等待线程 } close(clientSock); } close(listenSock); std::cout << "文件接收服务器已停止" << std::endl; } // 接收文件 bool FileTransfer::receiveFile(int sock, ReceivedFile& file) { // 1. 接收路径长度和路径 uint32_t pathLen; if (recv(sock, &pathLen, sizeof(pathLen), 0) != sizeof(pathLen)) { std::cerr << "接收路径长度失败" << std::endl; return false; } pathLen = ntohl(pathLen); std::vector<char> pathBuffer(pathLen + 1); ssize_t received = recv(sock, pathBuffer.data(), pathLen, 0); if (received != static_cast<ssize_t>(pathLen)) { std::cerr << "接收路径失败" << std::endl; return false; } pathBuffer[pathLen] = '\0'; file.filename = pathBuffer.data(); // 2. 接收文件长度 uint32_t fileSize; if (recv(sock, &fileSize, sizeof(fileSize), 0) != sizeof(fileSize)) { std::cerr << "接收文件长度失败" << std::endl; return false; } fileSize = ntohl(fileSize); // 3. 接收文件内容 file.content.resize(fileSize); ssize_t totalReceived = 0; while (totalReceived < static_cast<ssize_t>(fileSize) && m_running) { ssize_t bytes = recv(sock, file.content.data() + totalReceived, fileSize - totalReceived, 0); if (bytes <= 0) { if (bytes < 0) { std::cerr << "接收文件内容失败: " << strerror(errno) << std::endl; } return false; } totalReceived += bytes; } std::cout << "成功接收文件: " << file.filename << " (" << fileSize << " 字节)" << std::endl; return true; } // 等待文件到达 bool FileTransfer::waitForFile(ReceivedFile& file, int timeoutMs) { std::unique_lock<std::mutex> lock(m_mutex); // 只要有文件就立即返回 auto pred = [&] { return !m_receivedFiles.empty(); // 只需检查队列是否非空 }; if (timeoutMs > 0) { if (!m_cv.wait_for(lock, std::chrono::milliseconds(timeoutMs), pred)) { return false; // 超时 } } else { m_cv.wait(lock, pred); } if (m_receivedFiles.empty()) return false; file = std::move(m_receivedFiles.front()); m_receivedFiles.pop(); return true; } // 接收文件并读取两行内容 bool FileTransfer::receiveAndReadTwoStrings(int port, std::string& outStr1, std::string& outStr2, int timeoutMs) { // 确保服务器正在运行 if (!m_running) { startServer(port); } // 等待文件到达 ReceivedFile file; if (!waitForFile(file, timeoutMs)) { throw std::runtime_error("等待文件超时或服务器停止"); } // 读取文件内容 std::string content(file.content.data(), file.content.size()); // 查找第一行结尾 size_t pos = content.find('\n'); if (pos == std::string::npos) { // 没有换行符,整个内容作为第一行 outStr1 = content; outStr2 = ""; } else { // 提取第一行 outStr1 = content.substr(0, pos); // 提取第二行(跳过换行符) outStr2 = content.substr(pos + 1); // 移除第二行可能的多余换行符 if (!outStr2.empty() && outStr2.back() == '\n') { outStr2.pop_back(); } } return true; } bool Ucar::navigateTo(const geometry_msgs::Pose& target) { move_base_msgs::MoveBaseGoal goal; goal.target_pose.header.frame_id = "map"; goal.target_pose.pose = target; move_base_client_.sendGoal(goal); return move_base_client_.waitForResult(navigation_timeout_); } // void Ucar::recovery() { // setlocale(LC_ALL, ""); // std_srvs::Empty srv; // if (clear_costmaps_client_.call(srv)) { // ROS_INFO("代价地图清除成功"); // } else { // ROS_ERROR("代价地图清除服务调用失败"); // } // } Ucar::Ucar(ros::NodeHandle& nh) : nh_(nh), retry_count_(0), current_state_(State::INIT) , control_rate_(10) ,move_base_client_("move_base", true) { latest_odom_.reset(); // 等待move_base服务器就绪(参考网页6的actionlib使用规范) if(!move_base_client_.waitForServer(ros::Duration(5.0))) { setlocale(LC_ALL, ""); ROS_ERROR("无法连接move_base服务器"); } scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1); // 初始化代价地图清除服务 clear_costmaps_client_ = nh_.serviceClient<std_srvs::Empty>("/move_base/clear_costmaps"); setlocale(LC_ALL, ""); // 标记所有的点位总共21个点位 nh_.param("pose1_x", pose_1.position.x, 1.67); nh_.param("pose1_y", pose_1.position.y, 0.04); pose_1.orientation.x = 0.0; pose_1.orientation.y = 0.0; pose_1.orientation.z = 0.707; pose_1.orientation.w = 0.707; nh_.param("pose2_x", pose_2.position.x, 1.83); nh_.param("pose2_y", pose_2.position.y, 0.380); pose_2.orientation.x = 0.0; pose_2.orientation.y = 0.0; pose_2.orientation.z = 1; pose_2.orientation.w = 0; nh_.param("pose3_x", pose_3.position.x, 1.0); nh_.param("pose3_y", pose_3.position.y, 0.494); pose_3.orientation.x = 0.0; pose_3.orientation.y = 0.0; pose_3.orientation.z = 1; pose_3.orientation.w = 0; nh_.param("pose4_x", pose_4.position.x, 0.15166891130059032); nh_.param("pose4_y", pose_4.position.y, 0.5446138339072089); pose_4.orientation.x = 0.0; pose_4.orientation.y = 0.0; pose_4.orientation.z = 0.707; pose_4.orientation.w = 0.707; nh_.param("pose5_x", pose_5.position.x, 0.387605134459779); nh_.param("pose5_y", pose_5.position.y, 0.949243539748394); pose_5.orientation.x = 0.0; pose_5.orientation.y = 0.0; pose_5.orientation.z = 0.0; pose_5.orientation.w = 1.0; nh_.param("pose6_x", pose_6.position.x, 1.0250469329539987); nh_.param("pose6_y", pose_6.position.y, 1.0430107266961729); pose_6.orientation.x = 0.0; pose_6.orientation.y = 0.0; pose_6.orientation.z = 0.0; pose_6.orientation.w = 1.0; nh_.param("pose7_x", pose_7.position.x, 1.715746358650675); nh_.param("pose7_y", pose_7.position.y, 1.0451169673664757); pose_7.orientation.x = 0.0; pose_7.orientation.y = 0.0; pose_7.orientation.z = 0.707; pose_7.orientation.w = 0.707; nh_.param("pose8_x", pose_8.position.x, 1.820954899866641); nh_.param("pose8_y", pose_8.position.y, 1.405578846446346); pose_8.orientation.x = 0.0; pose_8.orientation.y = 0.0; pose_8.orientation.z = 1; pose_8.orientation.w = 0; nh_.param("pose9_x", pose_9.position.x, 1.287663212010699); nh_.param("pose9_y", pose_9.position.y, 1.4502232396357953); pose_9.orientation.x = 0.0; pose_9.orientation.y = 0.0; pose_9.orientation.z = 1; pose_9.orientation.w = 0; nh_.param("pose10_x", pose_10.position.x, 0.433025661760874); nh_.param("pose10_y", pose_10.position.y, 1.5362058814619577); pose_10.orientation.x = 0.0; pose_10.orientation.y = 0.0; pose_10.orientation.z = 0.707; pose_10.orientation.w = 0.707; //开始进入视觉定位区域 中心点加上四个中线点对应四面墙 nh_.param("pose_center_x", pose_11.position.x, 1.13); nh_.param("pose_center_y", pose_11.position.y, 3.04); nh_.param("pose_center_x", pose_12.position.x, 1.13); nh_.param("pose_center_y", pose_12.position.y, 3.04); nh_.param("pose_center_x", pose_13.position.x, 1.13); nh_.param("pose_center_y", pose_13.position.y, 3.04); nh_.param("pose_center_x", pose_14.position.x, 1.13); nh_.param("pose_center_y", pose_14.position.y, 3.04); nh_.param("pose_center_x", pose_15.position.x, 1.13); nh_.param("pose_center_y", pose_15.position.y, 3.04); //退出视觉区域进入路灯识别区域 nh_.param("pose_center_x", pose_16.position.x, 1.13); nh_.param("pose_center_y", pose_16.position.y, 3.04); nh_.param("pose_center_x", pose_17.position.x, 1.13); nh_.param("pose_center_y", pose_17.position.y, 3.04); nh_.param("pose_center_x", pose_18.position.x, 1.13); nh_.param("pose_center_y", pose_18.position.y, 3.04); //退出路灯识别区域,进入巡线区域 注意两个点位取一个,必须标定正确的方向 nh_.param("pose_center_x", pose_19.position.x, 1.13); nh_.param("pose_center_y", pose_19.position.y, 3.04); pose_19.orientation.x = 0.0; pose_19.orientation.y = 0.0; pose_19.orientation.z = 0.707; pose_19.orientation.w = 0.707; nh_.param("pose_center_x", pose_20.position.x, 1.13); nh_.param("pose_center_y", pose_20.position.y, 3.04); pose_20.orientation.x = 0.0; pose_20.orientation.y = 0.0; pose_20.orientation.z = 0.707; pose_20.orientation.w = 0.707; //result_sub_ = nh_.subscribe("result_of_object", 1, &Ucar::detectionCallback, this); result_client_ = nh_.serviceClient<rfbot_yolov8_ros::DetectGood>("object_realsense_recognization"); cmd_vel_pub = nh_.advertise<geometry_msgs::Twist>("/cmd_vel", 10); odom_sub = nh_.subscribe("/odom", 10, &Ucar::odomCallback, this); interaction_client = nh_.serviceClient<ucar::ObjectDetection>("object_detection"); ROS_INFO_STREAM("状态机初始化完成"); } void Ucar::navigate_to_aruco_place(){ setlocale(LC_ALL, ""); navigateTo(pose_1);ROS_INFO("到达pose_1"); navigateTo(pose_2);ROS_INFO("到达pose_2"); navigateTo(pose_3);ROS_INFO("到达pose_3"); } void Ucar::navigate_to_recongnition_place(){ setlocale(LC_ALL, ""); navigateTo(pose_4);ROS_INFO("到达pose_4"); navigateTo(pose_5);ROS_INFO("到达pose_5"); navigateTo(pose_6);ROS_INFO("到达pose_6"); navigateTo(pose_7);ROS_INFO("到达pose_7"); navigateTo(pose_8);ROS_INFO("到达pose_8"); navigateTo(pose_9);ROS_INFO("到达pose_9"); navigateTo(pose_10);ROS_INFO("到达pose_10"); } void Ucar::run() { while (ros::ok()) { ros::spinOnce(); switch(current_state_) { case State::INIT: {handleInit(); break;} case State::ARUCO: {navigate_to_aruco_place(); ROS_INFO("导航到二维码区域"); see_aruco(); if(target_object=="vegetable") playAudioFile("/home/ucar/ucar_ws/src/wav/sc.wav"); if(target_object=="fruit") playAudioFile("/home/ucar/ucar_ws/src/wav/sg.wav"); if(target_object=="dessert") playAudioFile("/home/ucar/ucar_ws/src/wav/tp.wav"); break;} case State::FIND: {navigate_to_recongnition_place(); spin_to_find();//转着找目标 break;} case State::GAZEBO: {gazebo(); break;} } control_rate_.sleep(); } } void Ucar::handleInit() { setlocale(LC_ALL, ""); ROS_DEBUG("执行初始化流程"); current_state_ = State::ARUCO; } void Ucar::see_aruco() { qr_found_ = false; image_transport::ImageTransport it(nh_); // 2. 创建订阅器,订阅/usb_cam/image_raw话题 image_transport::Subscriber sub = it.subscribe( "/usb_cam/image_raw", 1, &Ucar::imageCallback, this ); std::cout << "已订阅摄像头话题,等待图像数据..." << std::endl; // 3. 创建ZBar扫描器 scanner.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1); target_object = ""; // 重置结果 // 4. 设置超时检测 const int MAX_SCAN_DURATION = 30; // 最大扫描时间(秒) auto scan_start_time = std::chrono::steady_clock::now(); // 5. ROS事件循环 ros::Rate loop_rate(30); // 30Hz while (ros::ok() && !qr_found_) { // 检查超时 auto elapsed = std::chrono::steady_clock::now() - scan_start_time; if (std::chrono::duration_cast<std::chrono::seconds>(elapsed).count() > MAX_SCAN_DURATION) { std::cout << "扫描超时" << std::endl; break; } ros::spinOnce(); loop_rate.sleep(); } // 6. 清理资源 if (!qr_found_) target_object = ""; } void Ucar::imageCallback(const sensor_msgs::ImageConstPtr& msg) { try { // 7. 将ROS图像消息转换为OpenCV格式 cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8); cv::Mat frame = cv_ptr->image; cv::Mat gray; cv::cvtColor(frame, gray, cv::COLOR_BGR2GRAY); // 8. 准备ZBar图像 cv::Mat gray_cont = gray.clone(); zbar::Image image(gray.cols, gray.rows, "Y800", gray_cont.data, gray.cols * gray.rows); // 9. 扫描二维码 if (scanner.scan(image) >= 0) { for (zbar::Image::SymbolIterator symbol = image.symbol_begin(); symbol != image.symbol_end(); ++symbol) { // 获取原始结果 std::string rawResult = symbol->get_data(); // 在存储到全局变量前将首字母转换为小写 target_object = toLowerFirstChar(rawResult); ROS_INFO("识别到二维码: %s", target_object.c_str()); current_state_ = State::FIND; qr_found_ = true; // 在视频帧上绘制结果 std::vector<cv::Point> points; for (int i = 0; i < symbol->get_location_size(); i++) { points.push_back(cv::Point(symbol->get_location_x(i), symbol->get_location_y(i))); } // 绘制二维码边界框 cv::RotatedRect rect = cv::minAreaRect(points); cv::Point2f vertices[4]; rect.points(vertices); for (int i = 0; i < 4; i++) { cv::line(frame, vertices[i], vertices[(i + 1) % 4], cv::Scalar(0, 255, 0), 2); } // 显示原始二维码内容 cv::putText(frame, rawResult, cv::Point(10, 30), cv::FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0, 255, 0), 2); // 显示转换后的结果 cv::putText(frame, "Stored: " + target_object, cv::Point(10, 60), cv::FONT_HERSHEY_SIMPLEX, 0.7, cv::Scalar(0, 0, 255), 2); // 显示最终结果画面 cv::imshow("QR Code Scanner", frame); cv::waitKey(250); // 短暂显示结果 } } // 显示当前帧 cv::imshow("QR Code Scanner", frame); cv::waitKey(1); } catch (cv_bridge::Exception& e) { ROS_ERROR("cv_bridge异常: %s", e.what()); } } //播放音频文件函数 bool Ucar::playAudioFile(const std::string& filePath) { // 创建子进程播放音频 pid_t pid = fork(); if (pid == 0) { // 子进程 // 尝试使用 aplay (适用于 WAV 文件) execlp("aplay", "aplay", "-q", filePath.c_str(), (char*)NULL); // 如果 aplay 失败,尝试 ffplay execlp("ffplay", "ffplay", "-nodisp", "-autoexit", "-loglevel", "quiet", filePath.c_str(), (char*)NULL); // 如果 ffplay 失败,尝试 mpg123 (适用于 MP3) execlp("mpg123", "mpg123", "-q", filePath.c_str(), (char*)NULL); // 如果所有播放器都不可用,退出 exit(1); } else if (pid > 0) { // 父进程 int status; waitpid(pid, &status, 0); return WIFEXITED(status) && WEXITSTATUS(status) == 0; } else { // fork 失败 perror("fork failed"); return false; } } // 辅助函数:将字符串首字母转换为小写 std::string Ucar::toLowerFirstChar(const std::string& str) { if (str.empty()) return str; std::string result = str; result[0] = static_cast<char>(std::tolower(static_cast<unsigned char>(result[0]))); return result; } // 在odomCallback中更新最新里程计数据 void Ucar::odomCallback(const nav_msgs::Odometry::ConstPtr& msg) { latest_odom_ = msg; // 保存最新里程计数据 if (!position_recorded) { initial_position = msg->pose.pose.position; position_recorded = true; //ROS_INFO("初始位置记录完成: (%.3f, %.3f)", // initial_position.x, initial_position.y); } } void Ucar::moveLeftDistance(double distance_m, double linear_speed) { // 订阅里程计话题(根据实际机器人配置修改话题名) // 等待位置记录完成 position_recorded = false; while (!position_recorded && ros::ok()) { ros::spinOnce(); ros::Duration(0.1).sleep(); } // 计算理论运动时间 double duration = distance_m / linear_speed; //ROS_INFO("开始左移: 距离=%.2fm, 速度=%.2fm/s, 理论时间=%.1fs", // distance_m, linear_speed, duration); // 创建控制消息 geometry_msgs::Twist move_cmd; move_cmd.linear.y = linear_speed; // 全向移动Y轴速度控制左右移动 // 记录开始时间 auto start_time = ros::Time::now(); // 持续发送控制命令 while ((ros::Time::now() - start_time).toSec() < duration && ros::ok()) { cmd_vel_pub.publish(move_cmd); ros::Duration(0.05).sleep(); // 50ms控制周期 } // 发送停止指令 move_cmd.linear.y = 0; cmd_vel_pub.publish(move_cmd); //ROS_INFO("左移完成"); } void Ucar::moveRightDistance(double distance_m, double linear_speed) { // 等待位置记录完成 position_recorded = false; while (!position_recorded && ros::ok()) { ros::spinOnce(); ros::Duration(0.1).sleep(); } // 计算理论运动时间 double duration = distance_m / linear_speed; //ROS_INFO("开始右移: 距离=%.2fm, 速度=%.2fm/s, 理论时间=%.1fs", // distance_m, linear_speed, duration); // 创建控制消息 geometry_msgs::Twist move_cmd; move_cmd.linear.y = -linear_speed; // 全向移动Y轴速度控制左右移动 // 记录开始时间 auto start_time = ros::Time::now(); // 持续发送控制命令 while ((ros::Time::now() - start_time).toSec() < duration && ros::ok()) { cmd_vel_pub.publish(move_cmd); ros::Duration(0.05).sleep(); // 50ms控制周期 } // 发送停止指令 move_cmd.linear.y = 0; cmd_vel_pub.publish(move_cmd); //ROS_INFO("右移完成"); } void Ucar::moveForwardDistance(double distance_m, double linear_speed) { // 确保速度为正值(前进方向) if (linear_speed < 0) { ROS_WARN("前进速度应为正值,自动取绝对值"); linear_speed = fabs(linear_speed); } // 等待位置记录完成 position_recorded = false; while (!position_recorded && ros::ok()) { ros::spinOnce(); ros::Duration(0.1).sleep(); } // 计算理论运动时间 double duration = distance_m / linear_speed; //ROS_INFO("开始前进: 距离=%.2fm, 速度=%.2fm/s, 理论时间=%.1fs", // distance_m, linear_speed, duration); // 创建控制消息 geometry_msgs::Twist move_cmd; move_cmd.linear.x = linear_speed; // X轴速度控制前后移动 // 记录开始时间 auto start_time = ros::Time::now(); // 持续发送控制命令 while ((ros::Time::now() - start_time).toSec() < duration && ros::ok()) { cmd_vel_pub.publish(move_cmd); ros::Duration(0.05).sleep(); // 50ms控制周期 } // 发送停止指令 move_cmd.linear.x = 0.0; cmd_vel_pub.publish(move_cmd); ROS_INFO("前进完成"); } //输入1逆时针,输入-1顺时针 void Ucar::rotateCounterClockwise5Degrees(int a) { // 设置旋转参数 const double angular_speed = 0.2; // rad/s (约11.5度/秒) const double degrees = 5.0; const double duration = degrees * (M_PI / 180.0) / angular_speed; // 约0.436秒 //if(a==1) //ROS_INFO("开始逆时针旋转5度: 速度=%.2f rad/s, 时间=%.3f秒", // angular_speed, duration); //else if(a==-1) //ROS_INFO("开始顺时针旋转5度: 速度=%.2f rad/s, 时间=%.3f秒", // angular_speed, duration); // 创建控制消息 geometry_msgs::Twist twist_msg; twist_msg.angular.z = angular_speed*a; // 正值表示逆时针 // 记录开始时间 auto start_time = ros::Time::now(); // 持续发送控制命令 while ((ros::Time::now() - start_time).toSec() < duration && ros::ok()) { cmd_vel_pub.publish(twist_msg); ros::Duration(0.05).sleep(); // 20Hz控制周期 } // 发送停止命令(确保接收) twist_msg.angular.z = 0.0; for (int i = 0; i < 3; i++) { cmd_vel_pub.publish(twist_msg); ros::Duration(0.02).sleep(); } if(a==1) ROS_INFO("逆时针旋转5度完成"); else if (a==-1) ROS_INFO("顺时针旋转5度完成"); } void Ucar::rotateCounterClockwise360Degrees() { // 设置旋转参数 const double angular_speed = 1; // rad/s (约11.5度/秒) const double degrees = 360.0; const double duration = degrees * (M_PI / 180.0) / angular_speed; // 约0.436秒 // 创建控制消息 geometry_msgs::Twist twist_msg; twist_msg.angular.z = angular_speed; // 正值表示逆时针 // 记录开始时间 auto start_time = ros::Time::now(); // 持续发送控制命令 while ((ros::Time::now() - start_time).toSec() < duration && ros::ok()) { cmd_vel_pub.publish(twist_msg); ros::Duration(0.05).sleep(); // 20Hz控制周期 } // 发送停止命令(确保接收) twist_msg.angular.z = 0.0; for (int i = 0; i < 3; i++) { cmd_vel_pub.publish(twist_msg); ros::Duration(0.02).sleep(); } } void Ucar::left_and_right_move_old(){ rfbot_yolov8_ros::DetectGood srv; while((find_object_x2_old+find_object_x1_old)/2>324){ if((find_object_x2_old+find_object_x1_old)>340) moveLeftDistance(0.15,0.1);//控制小车往左移动15cm else moveLeftDistance(0.05,0.1);//控制小车往左移动5cm moveForwardDistance(0.05,0.1); Ucar::result_client_.call(srv); for (size_t j = 0; j < srv.response.goodName.size(); ++j) { ROS_INFO("响应结果:"); ROS_INFO("位置: x1: %d, y1: %d, x2: %d, y2: %d", srv.response.u1[j], srv.response.v1[j], srv.response.u2[j], srv.response.v2[j]); //ROS_INFO("置信度: %f", srv.response.confidence[j]); find_object_x1_old=srv.response.u1[0]; find_object_y1_old=srv.response.v1[0]; find_object_x2_old=srv.response.u2[0]; find_object_y2_old=srv.response.v2[0]; } ROS_INFO("当前目标中心横坐标为:%d",(find_object_x2_old+find_object_x1_old)/2); } while((find_object_x2_old+find_object_x1_old)/2<316){ if((find_object_x2_old+find_object_x1_old)<300) moveRightDistance(0.15,0.1);//控制小车往右移动15cm else moveRightDistance(0.05,0.1);//控制小车往右移动5cm moveForwardDistance(0.05,0.1); Ucar::result_client_.call(srv); for (size_t j = 0; j < srv.response.goodName.size(); ++j) { ROS_INFO("响应结果:"); ROS_INFO("位置: x1: %d, y1: %d, x2: %d, y2: %d", srv.response.u1[j], srv.response.v1[j], srv.response.u2[j], srv.response.v2[j]); //ROS_INFO("置信度: %f", srv.response.confidence[j]); find_object_x1_old=srv.response.u1[0]; find_object_y1_old=srv.response.v1[0]; find_object_x2_old=srv.response.u2[0]; find_object_y2_old=srv.response.v2[0]; } ROS_INFO("当前目标中心横坐标为:%d",(find_object_x2_old+find_object_x1_old)/2); } ROS_INFO("左右移动完成"); } //前进函数(涵盖第二种停靠算法) void Ucar::go(){ rfbot_yolov8_ros::DetectGood srv; while(1){ moveForwardDistance(0.05,0.1);//控制小车前进 Ucar::result_client_.call(srv); for (size_t j = 0; j < srv.response.goodName.size(); ++j) { ROS_INFO("响应结果:"); ROS_INFO("位置: x1: %d, y1: %d, x2: %d, y2: %d", srv.response.u1[j], srv.response.v1[j], srv.response.u2[j], srv.response.v2[j]); //ROS_INFO("置信度: %f", srv.response.confidence[j]); find_object_x1_new=srv.response.u1[0]; find_object_y1_new=srv.response.v1[0]; find_object_x2_new=srv.response.u2[0]; find_object_y2_new=srv.response.v2[0]; } //图像左边先到达边线——>逆时针往右 if(find_object_x2_new==640&&find_object_y1_new==0&&find_object_x1_new!=0&&find_object_y2_new>=350){ if(find_object_x1_new>240||(find_object_x2_new-find_object_x1_new)<=(find_object_y2_new-find_object_y1_new)){ rotateCounterClockwise5Degrees(1); rotateCounterClockwise5Degrees(1); rotateCounterClockwise5Degrees(1); rotateCounterClockwise5Degrees(1); rotateCounterClockwise5Degrees(1); moveRightDistance(0.35,0.1); ROS_INFO("大摆头"); break; } else if(find_object_x1_new>120){ rotateCounterClockwise5Degrees(1); rotateCounterClockwise5Degrees(1); rotateCounterClockwise5Degrees(1); moveRightDistance(0.25,0.1); ROS_INFO("中摆头"); break; } else{ rotateCounterClockwise5Degrees(1);//逆时针 moveRightDistance(0.1,0.1); ROS_INFO("小摆头"); break; } } //图像右边先到达边线——>顺时针往左 else if(find_object_x1_new==0&&find_object_y1_new==0&&find_object_x2_new!=640&&find_object_y2_new>=350){ if(find_object_x2_new<400||(find_object_x2_new-find_object_x1_new)<=(find_object_y2_new-find_object_y1_new)){ rotateCounterClockwise5Degrees(-1); rotateCounterClockwise5Degrees(-1); rotateCounterClockwise5Degrees(-1); rotateCounterClockwise5Degrees(-1); rotateCounterClockwise5Degrees(-1); moveLeftDistance(0.35,0.1); ROS_INFO("大摆头"); break; } else if(find_object_x2_new<520){ rotateCounterClockwise5Degrees(-1); rotateCounterClockwise5Degrees(-1); rotateCounterClockwise5Degrees(-1); moveLeftDistance(0.25,0.1); ROS_INFO("中摆头"); break; } else{ rotateCounterClockwise5Degrees(-1);//顺时针 moveLeftDistance(0.1,0.1); ROS_INFO("小摆头"); break; } } } } void Ucar::visualservo(){ rfbot_yolov8_ros::DetectGood srv; //左移右移 left_and_right_move_old(); //提取长宽比 Ucar::result_client_.call(srv); for (size_t j = 0; j < srv.response.goodName.size(); ++j) { ROS_INFO("响应结果:"); ROS_INFO("位置: x1: %d, y1: %d, x2: %d, y2: %d", srv.response.u1[j], srv.response.v1[j], srv.response.u2[j], srv.response.v2[j]); //ROS_INFO("置信度: %f", srv.response.confidence[j]); find_object_x1_old=srv.response.u1[0]; find_object_y1_old=srv.response.v1[0]; find_object_x2_old=srv.response.u2[0]; find_object_y2_old=srv.response.v2[0]; } changkuanbi_old=(find_object_x2_old-find_object_x1_old)/(find_object_y2_old-find_object_y1_old); ROS_INFO("长宽比为:%f",changkuanbi_old);go(); if(find_object=="dessert1") playAudioFile("/home/ucar/ucar_ws/src/wav/kl.wav"); if(find_object=="dessert2") playAudioFile("/home/ucar/ucar_ws/src/wav/dg.wav"); if(find_object=="dessert3") playAudioFile("/home/ucar/ucar_ws/src/wav/nn.wav"); if(find_object=="vegetable1") playAudioFile("/home/ucar/ucar_ws/src/wav/lj.wav"); if(find_object=="vegetable2") playAudioFile("/home/ucar/ucar_ws/src/wav/xhs.wav"); if(find_object=="vegetable3") playAudioFile("/home/ucar/ucar_ws/src/wav/td.wav"); if(find_object=="fruit1") playAudioFile("/home/ucar/ucar_ws/src/wav/xj.wav"); if(find_object=="fruit2") playAudioFile("/home/ucar/ucar_ws/src/wav/pg.wav"); if(find_object=="fruit3") playAudioFile("/home/ucar/ucar_ws/src/wav/xg.wav"); // if(abs(changkuanbi_old-1.666666667)<0.05){ // ROS_INFO("比较接近16:9,不需要旋转"); // //前进 // go(); // } // else { // //先逆时针转个10度 // rotateCounterClockwise5Degrees(1); // rotateCounterClockwise5Degrees(1); // Ucar::result_client_.call(srv); // for (size_t j = 0; j < srv.response.goodName.size(); ++j) { // ROS_INFO("响应结果:"); // ROS_INFO("位置: x1: %d, y1: %d, x2: %d, y2: %d", // srv.response.u1[j], // srv.response.v1[j], // srv.response.u2[j], // srv.response.v2[j]); // //ROS_INFO("置信度: %f", srv.response.confidence[j]); // find_object_x1_new=srv.response.u1[0]; // find_object_y1_new=srv.response.v1[0]; // find_object_x2_new=srv.response.u2[0]; // find_object_y2_new=srv.response.v2[0]; // } // changkuanbi_new=(find_object_x2_new-find_object_x1_new)/(find_object_y2_new-find_object_y1_new); // ROS_INFO("长宽比为:%f",changkuanbi_new); // if(changkuanbi_new<changkuanbi_old)//方向错了 // { // while(abs(changkuanbi_new-1.666666667)>0.4)//不准就再转 // { // rotateCounterClockwise5Degrees(-1); // Ucar::result_client_.call(srv); // for (size_t j = 0; j < srv.response.goodName.size(); ++j) { // ROS_INFO("响应结果:"); // ROS_INFO("位置: x1: %d, y1: %d, x2: %d, y2: %d", // srv.response.u1[j], // srv.response.v1[j], // srv.response.u2[j], // srv.response.v2[j]); // //ROS_INFO("置信度: %f", srv.response.confidence[j]); // find_object_x1_new=srv.response.u1[0]; // find_object_y1_new=srv.response.v1[0]; // find_object_x2_new=srv.response.u2[0]; // find_object_y2_new=srv.response.v2[0]; // //保持正对目标 // while((find_object_x2_new+find_object_x1_new)/2>324) // {moveLeftDistance(0.05,0.1);//控制小车往左移动5cm // Ucar::result_client_.call(srv); // for (size_t j = 0; j < srv.response.goodName.size(); ++j) { // ROS_INFO("响应结果:"); // ROS_INFO("位置: x1: %d, y1: %d, x2: %d, y2: %d", // srv.response.u1[j], // srv.response.v1[j], // srv.response.u2[j], // srv.response.v2[j]); // //ROS_INFO("置信度: %f", srv.response.confidence[j]); // find_object_x1_new=srv.response.u1[0]; // find_object_y1_new=srv.response.v1[0]; // find_object_x2_new=srv.response.u2[0]; // find_object_y2_new=srv.response.v2[0]; // } // changkuanbi_new=(find_object_x2_new-find_object_x1_new)/(find_object_y2_new-find_object_y1_new); // ROS_INFO("长宽比为:%f",changkuanbi_new);} // while((find_object_x2_new+find_object_x1_new)/2<316) // {moveRightDistance(0.05,0.1);//控制小车往右移动5cm // Ucar::result_client_.call(srv); // for (size_t j = 0; j < srv.response.goodName.size(); ++j) { // ROS_INFO("响应结果:"); // ROS_INFO("位置: x1: %d, y1: %d, x2: %d, y2: %d", // srv.response.u1[j], // srv.response.v1[j], // srv.response.u2[j], // srv.response.v2[j]); // //ROS_INFO("置信度: %f", srv.response.confidence[j]); // find_object_x1_new=srv.response.u1[0]; // find_object_y1_new=srv.response.v1[0]; // find_object_x2_new=srv.response.u2[0]; // find_object_y2_new=srv.response.v2[0]; // } // changkuanbi_new=(find_object_x2_new-find_object_x1_new)/(find_object_y2_new-find_object_y1_new); // ROS_INFO("长宽比为:%f",changkuanbi_new);} // } // } // } // else{//方向对了 // while(abs(changkuanbi_new-1.666666667)>0.4)//不准就再转 // { // rotateCounterClockwise5Degrees(1); // //保持正对目标 // while((find_object_x2_new+find_object_x1_new)/2>324) moveLeftDistance(0.05,0.1);//控制小车往左移动5cm // while((find_object_x2_new+find_object_x1_new)/2<316) moveRightDistance(0.05,0.1);//控制小车往右移动5cm // Ucar::result_client_.call(srv); // for (size_t j = 0; j < srv.response.goodName.size(); ++j) { // ROS_INFO("响应结果:"); // ROS_INFO("位置: x1: %d, y1: %d, x2: %d, y2: %d", // srv.response.u1[j], // srv.response.v1[j], // srv.response.u2[j], // srv.response.v2[j]); // //ROS_INFO("置信度: %f", srv.response.confidence[j]); // find_object_x1_new=srv.response.u1[0]; // find_object_y1_new=srv.response.v1[0]; // find_object_x2_new=srv.response.u2[0]; // find_object_y2_new=srv.response.v2[0]; // } // changkuanbi_new=(find_object_x2_new-find_object_x1_new)/(find_object_y2_new-find_object_y1_new); // ROS_INFO("长宽比为:%f",changkuanbi_new); // } // } // //前进 // go(); // } ROS_INFO("导航完成"); ros::Duration(3000).sleep(); current_state_ = State::GAZEBO; } void Ucar::spin_to_find(){ setlocale(LC_ALL, ""); for(int i=0;i<9;i++){ navigateTo(pose_center[i]);//导航到i号点位 ros::Duration(3).sleep();//停留3秒 rfbot_yolov8_ros::DetectGood srv; Ucar::result_client_.call(srv); for (size_t j = 0; j < srv.response.goodName.size(); ++j) { ROS_INFO("响应结果:"); ROS_INFO("位置: x1: %d, y1: %d, x2: %d, y2: %d", srv.response.u1[j], srv.response.v1[j], srv.response.u2[j], srv.response.v2[j]); //ROS_INFO("置信度: %f", srv.response.confidence[j]); find_object_x1_old=srv.response.u1[0]; find_object_y1_old=srv.response.v1[0]; find_object_x2_old=srv.response.u2[0]; find_object_y2_old=srv.response.v2[0]; pose_center_object[i] = srv.response.goodName[0]; //看看这个方向有没有目标 } if(pose_center_object[i].find(target_object)!=std::string::npos&&(find_object_x2_old-find_object_x1_old)>=(find_object_y2_old-find_object_y1_old)) {ROS_INFO("本次任务目标识别成功"); find_object=pose_center_object[i]; visualservo(); break;} else{ ROS_INFO("本次任务目标识别失败,尝试下一个目标"); continue; } } } void Ucar::gazebo(){ navigateTo(pose_gazebo); //主从机通信 FileTransfer ft("/home/ucar/ucar_ws/src/ucar"); ft.saveToFile(target_object, "target.txt"); //电脑的ip ft.sendTo("192.168.1.100", 8080, "/home/ucar/ucar_ws/src/ucar/target.txt", "/home/zzs/gazebo_test_ws/src/ucar2/target.txt"); try { // 示例3:等待接收文件并读取内容 std::cout << "等待接收文件..." << std::endl; ft.receiveAndReadTwoStrings(8080, gazebo_object, gazebo_room); std::cout << "接收到的内容:\n" << gazebo_object << "\n"<<gazebo_room << std::endl; } catch (const std::exception& e) { std::cerr << "错误: " << e.what() << std::endl;} ros::Duration(30000).sleep();//停留3秒 } int main(int argc, char** argv) { ros::init(argc, argv, "multi_room_navigation_node"); ros::NodeHandle nh; Ucar ucar(nh); ucar.run(); return 0; }修改上述代码,在nh_.param("pose_center_x", pose_11.position.x, 1.13); nh_.param("pose_center_y", pose_11.position.y, 3.04); nh_.param("pose_center_x", pose_12.position.x, 1.16); nh_.param("pose_center_y", pose_12.position.y, 2.04); nh_.param("pose_center_x", pose_13.position.x, 5.13); nh_.param("pose_center_y", pose_13.position.y, 5.04); nh_.param("pose_center_x", pose_14.position.x, 3.13); nh_.param("pose_center_y", pose_14.position.y, 2.04); nh_.param("pose_center_x", pose_15.position.x, 1.13); nh_.param("pose_center_y", pose_15.position.y, 3.04);视觉定位区域对应的五个点位中,添加一个函数,只有到达这五个点位的时候开始旋转,在到达其中一个点位的时候,开始旋转速度不用很快,每隔0.3秒停顿0.5秒的时间识别目标,在视觉到目标之后触发视觉定位前往目标,没有识别到目标前往下一个点位,在识别到目标之后不前往下一个点位,不要修改move_base,只针对这五个点添加函数
最新发布
08-09
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值