LC 48. Rotate Image 矩阵变换

本文介绍了一种有效的算法,用于在原地(不使用额外的空间)将一个 n x n 的二维矩阵顺时针旋转90度。通过两步操作实现:先进行矩阵转置,再对每行元素进行反转。

You are given an n x n 2D matrix representing an image.

Rotate the image by 90 degrees (clockwise).

Note:
You have to rotate the image in-place, which means you have to modify the input 2D matrix directly. DO NOT allocate another 2D matrix and do the rotation.

Example 1:

Given input matrix = 
[
  [1,2,3],
  [4,5,6],
  [7,8,9]
],

rotate the input matrix in-place such that it becomes:
[
  [7,4,1],
  [8,5,2],
  [9,6,3]
]

Example 2:

Given input matrix =
[
  [ 5, 1, 9,11],
  [ 2, 4, 8,10],
  [13, 3, 6, 7],
  [15,14,12,16]
], 

rotate the input matrix in-place such that it becomes:
[
  [15,13, 2, 5],
  [14, 3, 4, 1],
  [12, 6, 8, 9],
  [16, 7,10,11]
]

先考虑这样一个问题,对于 n 维矩阵中坐标为  ( i , j ) 的点,经过题中所述的变换,新的坐标是多少呢?问题转化为求变换坐标函数 f ( i, j, n). 回想高中的数学题,描述一个旋转图形得到的点的坐标,有一种方法就是变换坐标系,在这里就是旋转坐标系。正常的平面直角坐标系中的图形经过题目中所述的变换,其实就相当于将直角坐标系经过如下旋转,从Oxy变换到Ox'y':


从图中不难看出,变换函数为 f ( x , y , n ) = ( y , n - x ). 因为n维矩阵的下标为 0 .. n - 1, 所以矩阵中的变换函数为 f ( i , j , n ) = ( j , n - i - 1 )。那么怎么用代码操作又方便有快速的实现对矩阵中所有点的转换呢?第一眼没有发现规律的话只能用笨办法,我一开始就是那么做的,代码比较复杂也不容易看到,这里简单介绍一下我解法一的代码:

void rotate(vector<vector<int>>& matrix) {
    int n = (int)matrix[0].size();
    int row, col, nextRow, nextCol, old, pre, count=0;
    while(n > 1) {
        for (int i=0; i<n-1; i++) {
            row =  0;
            col =  i;
            pre = matrix[count + row][count + col];
            for (int j=0; j<4; j++) {
                nextRow = col;
                nextCol = n - row - 1;
                old = matrix[count + nextRow][count + nextCol];
                matrix[count + nextRow][count + nextCol] = pre;
                cout<<count+nextRow<<" "<<count+nextCol<<endl;
                pre = old;
                row = nextRow;
                col = nextCol;
            }
        }
        n-=2;
        count++;
    }
}

这段代码实现的我一开始很直接的想法,就是从矩阵最外圈开始逐层往里变换。n -= 2控制了每一层的矩阵维度,count用来作为坐标的基准调整,因为变换函数对于内圈的坐标是无效的,必须模拟每次内圈在没有外圈的情况下的坐标变换情况再加上count作为补偿得到真正的坐标,这也就是最内圈循环中有 count + row 之类的操作。最内层的循环走4步,等于将最外圈对应位置的点轮转一圈。第一个 for 循环控制选择同一行上的点作为循环的起始点。pre 和 old 的类似对单向链表的操作实现单向循环。


解法二我是在看了LC别人的做法之后明白的,原作者没有写这么做的原因,但是有了上面的变换函数可以很轻松的理解。代码:

void rotate(vector<vector<int>>& m) {
        int n = m.size();
        
        for(int i=0; i<n; i++)
            for(int j=0; j<i; j++)
                swap(m[i][j], m[j][i]);
        
        for(int i=0; i<n; i++)
            reverse(m[i].begin(), m[i].end());
    }
首先,
swap(m[i][j], m[j][i])
这行代码的含义是求矩阵的转置,也就是完成 ( i , j ) -> ( j , i )。接下来为了将函数继续变换成 ( j , n - i - 1 ),仔细观察 n - i - 1 是如何在代码中体现的,就是对所有行完成倒置,因为列数为 i 的点在n维向量中经过reverse之后列数就变成了 n - i - 1。经历了这番思考,我终于明白了这段代码的意义,也给了我一个启发,在处理变换问题的时候有时可以高度依赖数学方法,先求出变换坐标的函数,然后用常见的变换方式组合实现目标变换,例如这里用到了转置和逐行倒置。通过这道题我感受到了数学和代码的结合的美感,真的很有意思。
#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),is_find(false),aucro_yes_or_no(false) { 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, ""); //20 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.283025661760874); 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("pose11_x", pose_11.position.x, 0.89); nh_.param("pose11_y", pose_11.position.y, 2.375); pose_11.orientation.x = 0.0; pose_11.orientation.y = 0.0; pose_11.orientation.z = 0.707; pose_11.orientation.w = 0.707; nh_.param("pose12_x", pose_12.position.x, 0.89); nh_.param("pose12_y", pose_12.position.y, 2.4775); pose_12.orientation.x = 0.0; pose_12.orientation.y = 0.0; pose_12.orientation.z = -0.707; pose_12.orientation.w = 0.707; nh_.param("pose13_x", pose_13.position.x, 0.89); nh_.param("pose13_y", pose_13.position.y, 3.9725); pose_13.orientation.x = 0.0; pose_13.orientation.y = 0.0; pose_13.orientation.z = 0.707; pose_13.orientation.w = 0.707; nh_.param("pose14_x", pose_14.position.x, 1.21775); nh_.param("pose14_y", pose_14.position.y, 3.225); pose_14.orientation.x = 0.0; pose_14.orientation.y = 0.0; pose_14.orientation.z = 0; pose_14.orientation.w = 1.0; nh_.param("pose15_x", pose_15.position.x, 0.5295325); nh_.param("pose15_y", pose_15.position.y, 3.225); pose_15.orientation.x = 0.0; pose_15.orientation.y = 0.0; pose_15.orientation.z = 1.0; pose_15.orientation.w = 0; //退出视觉定位区域进入路灯识别区域,路灯识别过程中是有角度要求的 //在多次测验中发现该地图大致逆时针偏向约60度,因此要消除60度的误差,因为此时地图的建模不标准 nh_.param("pose16_x", pose_16.position.x, 2.07); nh_.param("pose16_y", pose_16.position.y, 3.78); pose_16.orientation.x = -0.5; pose_16.orientation.y = 0.0; pose_16.orientation.z = 0; pose_16.orientation.w = 0.866; nh_.param("pose17_x", pose_17.position.x, 2.37); nh_.param("pose17_y", pose_17.position.y, 3.78); pose_17.orientation.x = -0.3536; pose_17.orientation.y = 0.3536; pose_17.orientation.z = 0.6124; pose_17.orientation.w = 0.6124; nh_.param("pose18_x", pose_18.position.x, 4.09); nh_.param("pose18_y", pose_18.position.y, 3.78); pose_18.orientation.x = -0.3536; pose_18.orientation.y = 0.3536; pose_18.orientation.z = 0.6124; pose_18.orientation.w = 0.6124; //退出路灯识别区域进入巡线区域,注意两个点位取一个,有方向要求 nh_.param("pose19_x", pose_19.position.x, 2.27); nh_.param("pose19_y", pose_19.position.y, 2.93); pose_19.orientation.x = 0; pose_19.orientation.y = 0; pose_19.orientation.z = 0; pose_19.orientation.w = 1.0; nh_.param("pose18_x", pose_20.position.x, 4.29); nh_.param("pose18_y", pose_20.position.y, 2.93); pose_20.orientation.x = 0; pose_20.orientation.y = 0; pose_20.orientation.z = 0; pose_20.orientation.w = 1.0; //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"); current_state_=State::FIND2; } //在视觉定位区域中转,找到目标 void Ucar::navigate_to_recongnition_place2(){ setlocale(LC_ALL, ""); navigateTo(pose_15);ROS_INFO("到达pose_15"); spin_to_find(); if(!is_find){ navigateTo(pose_12);ROS_INFO("到达pose_12"); spin_to_find(); } if(!is_find){ navigateTo(pose_13);ROS_INFO("到达pose_13"); spin_to_find(); } if(!is_find){ navigateTo(pose_14);ROS_INFO("到达pose_14"); spin_to_find(); } if(!is_find){ navigateTo(pose_11);ROS_INFO("到达pose_11"); spin_to_find(); } current_state_ = State::GAZEBO; } //主从机交互阶段 void Ucar::navigate_to_recongnition_place3(){ setlocale(LC_ALL, ""); navigateTo(pose_11);ROS_INFO("到达pose_11"); } //退出视觉定位区域进入路灯识别区域 void Ucar::navigate_to_recongnition_place4(){ setlocale(LC_ALL, ""); navigateTo(pose_16);ROS_INFO("到达pose_16"); navigateTo(pose_17);ROS_INFO("到达pose_17"); rfbot_yolov8_ros::DetectGood srv; Ucar::result_client_.call(srv); for (size_t j = 0; j < srv.response.goodName.size(); ++j) { ROS_INFO("响应结果:"); light = srv.response.goodName[0]; } if(light=="red") {ans=0;navigateTo(pose_18);ROS_INFO("到达pose_18");playAudioFile("/home/ucar/ucar_ws/src/wav/2.wav");} else if(light=="green") {ans=1;playAudioFile("/home/ucar/ucar_ws/src/wav/1.wav");} current_state_ = State::LINE; } //退出路灯识别区域进入巡线区域 void Ucar::navigate_to_recongnition_place5(){ setlocale(LC_ALL, ""); if(ans==1) {navigateTo(pose_19);ROS_INFO("到达pose_19");} else if(ans==0) {navigateTo(pose_20);ROS_INFO("到达pose_20");} } 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("导航到二维码区域"); while(!aucro_yes_or_no){ see_aruco(); moveForwardDistance(0.05,0.1); } 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(); break;} //在视觉定位区域中转,找到目标 case State::FIND2: {navigate_to_recongnition_place2();break;} //主从机交互阶段 case State::GAZEBO: {navigate_to_recongnition_place3(); gazebo(); break;} //退出视觉定位区域进入路灯识别区域 case State::LIGHT: {navigate_to_recongnition_place4(); break; } //退出路灯识别区域进入巡线区域 case State::LINE: {navigate_to_recongnition_place5(); 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 = ""; aucro_yes_or_no=true; } 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 Ucar::result_client_.call(srv); for (size_t j = 0; j < srv.response.goodName.size(); ++j) { if(srv.response.goodName[j].find(target_object)!=std::string::npos){ 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[j]; find_object_y1_old=srv.response.v1[j]; find_object_x2_old=srv.response.u2[j]; find_object_y2_old=srv.response.v2[j]; } else continue; } ROS_INFO("当前目标中心横坐标为:%.1f",(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 Ucar::result_client_.call(srv); for (size_t j = 0; j < srv.response.goodName.size(); ++j) { if(srv.response.goodName[j].find(target_object)!=std::string::npos){ 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[j]; find_object_y1_old=srv.response.v1[j]; find_object_x2_old=srv.response.u2[j]; find_object_y2_old=srv.response.v2[j]; } else continue; } ROS_INFO("当前目标中心横坐标为:%.1f",(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) { if(srv.response.goodName[j].find(target_object)!=std::string::npos){ 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[j]; find_object_y1_new=srv.response.v1[j]; find_object_x2_new=srv.response.u2[j]; find_object_y2_new=srv.response.v2[j]; } else continue; } if(find_object_x1_new<=10&&find_object_y1_new<=10&&find_object_x2_new>=630&&find_object_y2_new>=360){ ROS_INFO("靠中间,不白头"); break; } //图像左边先到达边线——>逆时针往右 else if(find_object_x2_new>=630&&find_object_y1_new<=10&&find_object_x1_new!=0&&find_object_y2_new>=350){ if(find_object_x1_new>240&&find_object_y2_new>360){ rotateCounterClockwise5Degrees(1); rotateCounterClockwise5Degrees(1); rotateCounterClockwise5Degrees(1); rotateCounterClockwise5Degrees(1); rotateCounterClockwise5Degrees(1); rotateCounterClockwise5Degrees(1); rotateCounterClockwise5Degrees(1); moveRightDistance(0.45,0.1); ROS_INFO("超级大摆头"); } else if(find_object_x1_new>240||(find_object_x2_new-find_object_x1_new)<=(find_object_y2_new-find_object_y1_new)||(find_object_y2_new<375&&find_object_x1_new>130)||(find_object_y2_new-find_object_x1_new>130)){ 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||(find_object_y2_new<360&&find_object_x1_new>65)||(find_object_y2_new>360&&find_object_x1_new>110)){ 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<=10&&find_object_y1_new<=10&&find_object_x2_new!=640&&find_object_y2_new>=350){ if(find_object_x2_new<400&&find_object_y2_new>360){ rotateCounterClockwise5Degrees(-1); rotateCounterClockwise5Degrees(-1); rotateCounterClockwise5Degrees(-1); rotateCounterClockwise5Degrees(-1); rotateCounterClockwise5Degrees(-1); rotateCounterClockwise5Degrees(-1); rotateCounterClockwise5Degrees(-1); moveLeftDistance(0.45,0.1); ROS_INFO("超级大摆头"); } else if(find_object_x2_new<400||(find_object_x2_new-find_object_x1_new)<=(find_object_y2_new-find_object_y1_new)||(find_object_y2_new<345&&find_object_x2_new<510)||(find_object_y2_new-find_object_x2_new<130)){ 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||(find_object_y2_new<360&&find_object_x2_new<585)||(find_object_y2_new>360&&find_object_x2_new<530)){ 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(){ is_find=true; rfbot_yolov8_ros::DetectGood srv; Ucar::result_client_.call(srv); for (size_t j = 0; j < srv.response.goodName.size(); ++j) { if(srv.response.goodName[j].find(target_object)!=std::string::npos){ 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[j]; find_object_y1_old=srv.response.v1[j]; find_object_x2_old=srv.response.u2[j]; find_object_y2_old=srv.response.v2[j]; } else continue; } //左移右移 left_and_right_move_old(); //提取长宽比 Ucar::result_client_.call(srv); for (size_t j = 0; j < srv.response.goodName.size(); ++j) { if(srv.response.goodName[j].find(target_object)!=std::string::npos){ 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[j]; find_object_y1_old=srv.response.v1[j]; find_object_x2_old=srv.response.u2[j]; find_object_y2_old=srv.response.v2[j]; } else continue; } 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(3).sleep(); current_state_ = State::GAZEBO; } void Ucar::spin_to_find(){ setlocale(LC_ALL, ""); //旋转扫描参数 const double rotation_speed=2.0; const double rotation_duration=2;//每次旋转时间 const double pause_duration=0.6;//move_base暂停时间 double total_rotation=0.0;//总旋转角度 //开始旋转 while(total_rotation<12.56){ geometry_msgs::Twist twist_cmd; twist_cmd.angular.z=rotation_speed; ros::Time start_rotate=ros::Time::now(); while ((ros::Time::now()-start_rotate).toSec()<rotation_duration) { cmd_vel_pub.publish(twist_cmd); ros::Duration(0.5).sleep(); } twist_cmd.angular.z=0.0; cmd_vel_pub.publish(twist_cmd); total_rotation+=rotation_speed*rotation_duration; //停顿进行目标的检测 ros::Time start_pause = ros::Time::now(); while ((ros::Time::now()-start_pause).toSec()<pause_duration) { rfbot_yolov8_ros::DetectGood srv; if(result_client_.call(srv)) { for(size_t i=0;i<srv.response.goodName.size();i++){ find_object_x1_old=srv.response.u1[i]; find_object_y1_old=srv.response.v1[i]; find_object_x2_old=srv.response.u2[i]; find_object_y2_old=srv.response.v2[i]; if(srv.response.goodName[i].find(target_object)!=std::string::npos&&((find_object_x2_old-find_object_x1_old)>90)){ ROS_INFO("目标检测成功"); find_object=srv.response.goodName[i]; //进入视觉定位函数 visualservo(); return; } else continue; } } ros::Duration(0.1).sleep(); } ROS_INFO("导航失败"); } } void Ucar::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(3).sleep();//停留3秒 current_state_ = State::LIGHT; } int main(int argc, char** argv) { ros::init(argc, argv, "multi_room_navigation_node"); ros::NodeHandle nh; Ucar ucar(nh); ucar.run(); return 0; }仿真任务结束后不进行下面阶段,帮我修改一下,同时解决在前往15,12,13,14等等电位时在这些点位上进行旋转时候导航发生了严重的偏移,帮我修改一下,给出解决上述两个问题的完整代码
最新发布
08-09
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值