ros(1-1) 影像和GPS釋出節點 c++版本

MKT-porter發表於2024-08-23

執行指令

#=============檔案結構
ros_cgg/
├── build
├── devel
└── src
    └── image_gps_package
        ├── CMakeLists.txt
        ├── package.xml
        └── src
            ├── image_gps_publisher.cpp
            └── image_gps_subscriber.cpp

#==============編譯
cd ~/ros_cgg
catkin_make
source devel/setup.bash


#==============手動執行
#0開啟source確保找到
cd ~/ros_cgg
source devel/setup.bash
# 手動開啟三個視窗
#1啟動ROS Master：
roscore
#2執行你的C++釋出節點：
rosrun image_gps_package image_gps_publisher
#3執行c++接受節點
rosrun image_gps_package image_gps_subscriber


#============ 指令碼執行
sudo chmod +x run_ros_nodes.sh

執行指令碼

run_ros_nodes.sh

#!/bin/bash

#外部給與執行許可權
#sudo chmod +x run_ros_nodes.sh

# 定義 ROS 安裝路徑  #安裝時候新增到系統路徑了 不需要每次都source
ROS_SETUP="/opt/ros/noetic/setup.bash" 
# 定義工作目錄路徑  自己的工程沒有加到系統路徑，每次需要source
WORKSPACE_DIR="/home/dongdong/2project/1salm/2Mycode/ros/ros_cgg"


#指定目錄

# 啟動 ROS Master
echo "Starting ROS 總結點..."
gnome-terminal -- bash -c "\
cd $WORKSPACE_DIR; source devel/setup.bash; \
roscore; \
exec bash"

# 等待 ROS Master 啟動
sleep 5

# 執行 C++ 釋出節點
echo "Running C++ 釋出節點..."
gnome-terminal -- bash -c "\
cd $WORKSPACE_DIR; source devel/setup.bash; \
rosrun image_gps_package image_gps_publisher; \
exec bash"

# 執行 C++ 接受節點
echo "Running C++ 訂閱節點..."
gnome-terminal -- bash -c "\
cd $WORKSPACE_DIR; source devel/setup.bash; \
rosrun image_gps_package image_gps_subscriber; \
exec bash"

1建立工程

CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(image_gps_package)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  sensor_msgs
  cv_bridge
)

find_package(OpenCV REQUIRED)
find_package(Boost REQUIRED COMPONENTS filesystem)

catkin_package(
  CATKIN_DEPENDS roscpp sensor_msgs cv_bridge
  DEPENDS Boost
)

include_directories(
  ${catkin_INCLUDE_DIRS}
  ${OpenCV_INCLUDE_DIRS}
  ${Boost_INCLUDE_DIRS}
)

# 編譯釋出節點
add_executable(image_gps_publisher src/image_gps_publisher.cpp)
target_link_libraries(image_gps_publisher
  ${catkin_LIBRARIES}
  ${OpenCV_LIBRARIES}
  ${Boost_LIBRARIES}
)

# 編譯接受節點
add_executable(image_gps_subscriber src/image_gps_subscriber.cpp)
target_link_libraries(image_gps_subscriber
  ${catkin_LIBRARIES}
  ${OpenCV_LIBRARIES}
  ${Boost_LIBRARIES}
)

　　package.xml

<?xml version="1.0"?>
<package format="2">
  <name>image_gps_package</name>
  <version>0.0.1</version>
  <description>
    A package to publish and subscribe to images and GPS data using ROS.
  </description>

  <!-- Maintainer of the package -->
  <maintainer email="your_email@example.com">Your Name</maintainer>

  <!-- License of the package -->
  <license>MIT</license>

  <!-- Build tool required to build this package -->
  <buildtool_depend>catkin</buildtool_depend>

  <!-- Dependencies of the package during build and runtime -->
  <build_depend>roscpp</build_depend>
  <build_depend>sensor_msgs</build_depend>
  <build_depend>cv_bridge</build_depend>
  
  <exec_depend>roscpp</exec_depend>
  <exec_depend>sensor_msgs</exec_depend>
  <exec_depend>cv_bridge</exec_depend>

  <!-- Declare additional dependencies required for building this package -->
  <build_export_depend>roscpp</build_export_depend>
  <build_export_depend>sensor_msgs</build_export_depend>
  <build_export_depend>cv_bridge</build_export_depend>

  <!-- Export information, can be used by other packages -->
  <export>
    <!-- Export any specific information here -->
  </export>
</package>

image_gps_publisher（復件）.cpp

單執行緒釋出

image_gps_publisher.cpp

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/NavSatFix.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <boost/filesystem.hpp>
#include <fstream>
#include <sstream>
#include <iostream>

class ImageGPS_Reader {


public:

    std::vector<boost::filesystem::path> image_files_;
    struct GPSData {
        std::string timestamp;
        double lat, lon, alt;
    };
    std::map<std::string, GPSData> gps_data_;


    ImageGPS_Reader() {
        std::string data_dir="/home/dongdong/2project/0data/NWPU/";
        std::string gps_dir=data_dir+"/config/gps.txt";
        std::string img_dir=data_dir+"img";
        // Load GPS data
        loadGPSData(gps_dir);

        // Load image filenames
        loadImageFilenames(img_dir);
    }

    void loadGPSData(const std::string& gps_file) {
        std::ifstream file(gps_file);
        if (!file.is_open()) {
            ROS_ERROR("Failed to open GPS file: %s", gps_file.c_str());
            return;
        }

        std::string line;
        while (std::getline(file, line)) {
            std::istringstream iss(line);
            std::string timestamp;
            double lat, lon, alt;
            if (!(iss >> timestamp >> lat >> lon >> alt)) { break; }
            gps_data_[timestamp] = {timestamp,lat, lon, alt};
        }
        file.close();
    }

    void loadImageFilenames(const std::string& img_folder) {
        namespace fs = boost::filesystem;
        fs::directory_iterator end_itr;
        for (fs::directory_iterator itr(img_folder); itr != end_itr; ++itr) {
            if (fs::is_regular_file(itr->status()) && itr->path().extension() == ".jpg") {
                image_files_.push_back(itr->path());
            }
        }
    }



};

int main(int argc, char** argv) {
    ros::init(argc, argv, "image_gps_publisher");

    ImageGPS_Reader reader;
    
    // Initialize ROS node handle
    ros::NodeHandle nh;
    // Create publishers
    ros::Publisher image_pub_ = nh.advertise<sensor_msgs::Image>("/camera/image_raw", 10);
    ros::Publisher gps_pub_ = nh.advertise<sensor_msgs::NavSatFix>("/gps/fix", 10);


    ros::Rate rate(10);  // 10 Hz

    while (ros::ok()) {
        if (!reader.image_files_.empty() && !reader.gps_data_.empty()) {
            for (const auto& img_file : reader.image_files_) {
                std::string timestamp = img_file.stem().string();  // Extract timestamp from filename
                
                if (reader.gps_data_.find(timestamp) != reader.gps_data_.end()) {// 確保GNSS對應影像存在
                    std::string img_path = img_file.string();
                    cv::Mat cv_image = cv::imread(img_path, cv::IMREAD_COLOR);
                    if (!cv_image.empty()) {
                        // 釋出影像

                        sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", cv_image).toImageMsg();
                        msg->header.stamp = ros::Time(std::stod(timestamp));                
                        image_pub_.publish(msg);

                        // 釋出GNSS資訊
                        auto gps = reader.gps_data_[timestamp];
                        sensor_msgs::NavSatFix gps_msg;
                        gps_msg.header.stamp = ros::Time(std::stod(timestamp));                      
                        gps_msg.latitude = gps.lat;
                        gps_msg.longitude = gps.lon;
                        gps_msg.altitude = gps.alt;
                        gps_pub_.publish(gps_msg);

                        ROS_INFO("Published image: %s and GPS data", img_file.filename().string().c_str());
                        rate.sleep(); // 按照設定的頻率休眠
                    }
                }
            }
        }
        //處理一次所有的回撥函式。這個呼叫會處理所有的回撥並清空訊息佇列。對於訊息處理來說，它確保了訊息的及時處理。
        ros::spinOnce();
    }//while

    return 0;
}

多執行緒釋出

如果用的時間是釋出當前時間還可以對齊，但是時間是歷史時間而非系統時間，對齊有問題

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/NavSatFix.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <boost/filesystem.hpp>
#include <fstream>
#include <sstream>
#include <iostream>
#include <thread>
#include <mutex>

class ImageGPS_Reader {
public:
    std::vector<boost::filesystem::path> image_files_;
    struct GPSData {
        std::string timestamp;
        double lat, lon, alt;
    };
    std::map<std::string, GPSData> gps_data_;

    ImageGPS_Reader() {
        std::string data_dir = "/home/dongdong/2project/0data/NWPU/";
        std::string gps_dir = data_dir + "/config/gps.txt";
        std::string img_dir = data_dir + "img";
        // Load GPS data
        loadGPSData(gps_dir);

        // Load image filenames
        loadImageFilenames(img_dir);
    }

    void loadGPSData(const std::string& gps_file) {
        std::ifstream file(gps_file);
        if (!file.is_open()) {
            ROS_ERROR("Failed to open GPS file: %s", gps_file.c_str());
            return;
        }

        std::string line;
        while (std::getline(file, line)) {
            std::istringstream iss(line);
            std::string timestamp;
            double lat, lon, alt;
            if (!(iss >> timestamp >> lat >> lon >> alt)) { break; }
            gps_data_[timestamp] = {timestamp, lat, lon, alt};
        }
        file.close();
    }

    void loadImageFilenames(const std::string& img_folder) {
        namespace fs = boost::filesystem;
        fs::directory_iterator end_itr;
        for (fs::directory_iterator itr(img_folder); itr != end_itr; ++itr) {
            if (fs::is_regular_file(itr->status()) && itr->path().extension() == ".jpg") {
                image_files_.push_back(itr->path());
            }
        }
    }
};

// Global variables for shared state and mutex
ImageGPS_Reader reader;
ros::Publisher image_pub_;
ros::Publisher gps_pub_;
std::mutex mtx;

void publishImages() {
    
    std::vector<boost::filesystem::path> image_files_copy;
    {
        std::lock_guard<std::mutex> lock(mtx);
        image_files_copy = reader.image_files_;
    }

    ros::Rate rate(10);  // 10 Hz
    while (ros::ok()) {
        for (const auto& img_file : image_files_copy) {
            std::string timestamp = img_file.stem().string();  // Extract timestamp from filename
            
            cv::Mat cv_image = cv::imread(img_file.string(), cv::IMREAD_COLOR);
            if (!cv_image.empty()) {
                // 釋出影像
                sensor_msgs::ImagePtr msg = cv_bridge::CvImage(std_msgs::Header(), "bgr8", cv_image).toImageMsg();
                msg->header.stamp = ros::Time(std::stod(timestamp));                
                image_pub_.publish(msg);
                ROS_INFO("Published image: %s", img_file.filename().string().c_str());
            }
            rate.sleep();
        }
    }
}

void publishGPSData() {

    std::map<std::string, ImageGPS_Reader::GPSData> gps_data_copy;
    {
        std::lock_guard<std::mutex> lock(mtx);
        gps_data_copy = reader.gps_data_;
    }

    ros::Rate rate(10);  // 10 Hz
    while (ros::ok()) {

        for (const auto& entry : gps_data_copy) {
            const auto& timestamp = entry.first;
            const auto& gps = entry.second;

            // 釋出GNSS資訊
            sensor_msgs::NavSatFix gps_msg;
            gps_msg.header.stamp = ros::Time(std::stod(timestamp));                      
            gps_msg.latitude = gps.lat;
            gps_msg.longitude = gps.lon;
            gps_msg.altitude = gps.alt;
            gps_pub_.publish(gps_msg);
            ROS_INFO("Published GPS data for timestamp: %s", timestamp.c_str());
            rate.sleep();
        }
    }
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "image_gps_publisher");
    ros::NodeHandle nh;

    // Create publishers
    image_pub_ = nh.advertise<sensor_msgs::Image>("/camera/image_raw", 10);
    gps_pub_ = nh.advertise<sensor_msgs::NavSatFix>("/gps/fix", 10);

    // Start threads for publishing images and GPS data
    std::thread image_thread(publishImages);
    std::thread gps_thread(publishGPSData);

    // Wait for threads to finish (they run indefinitely in this example)
    image_thread.join();
    gps_thread.join();

    return 0;
}

訂閱節點

image_gps_subscriber.cpp

#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/NavSatFix.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <deque>
#include <utility>

std::deque<std::pair<cv::Mat, ros::Time>> image_queue;
std::queue<sensor_msgs::NavSatFixConstPtr> gpsQueue;
std::mutex gpsQueue_Lock;// 佇列鎖

void gpsCallback(const sensor_msgs::NavSatFixConstPtr& gps_msg) {
    gpsQueue_Lock.lock();
    gpsQueue.push(gps_msg);
    gpsQueue_Lock.unlock();
}


void imageCallback(const sensor_msgs::ImageConstPtr& image_msg) {
    // Convert ROS image message to OpenCV image
    cv_bridge::CvImagePtr cv_image;
    try {
        cv_image = cv_bridge::toCvCopy(image_msg, sensor_msgs::image_encodings::BGR8);
    } catch (cv_bridge::Exception& e) {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
    
    // Get the timestamp of the image
    double image_timestamp = image_msg->header.stamp.toSec();

    // Store image and timestamp in the queue
    image_queue.emplace_back(cv_image->image, image_timestamp);

    
    gpsQueue_Lock.lock();

    while(!gpsQueue.empty()){

        sensor_msgs::NavSatFixConstPtr GPS_msg = gpsQueue.front();
        double gps_timestamp = GPS_msg->header.stamp.toSec();
        double time_diffrence = image_timestamp - gps_timestamp;
        printf("img t: %f, gps t: %f , time_diffrence: %f \n", image_timestamp, gps_timestamp,time_diffrence);
       

        if(abs(image_timestamp - gps_timestamp)<=0.01){// 影像和gps時間差在0.01s=10ms內的算匹配成功 vins-fusion參考值
            // 線上階段，假設影像20幀/S 相鄰幀間隔50ms 中間值間隔25ms算閾值考慮。
            // 離線階段，一般會提前手動處理影像和GPS時間戳對齊，影像名字以時間戳儲存，GNSS的第一列也是一樣的時間戳。

            // 經緯度、海拔
            double latitude = GPS_msg->latitude;
            double longitude = GPS_msg->longitude;
            double altitude = GPS_msg->altitude;
            gpsQueue.pop();// 使用完畢 丟棄
            
            std::cout << std::fixed << std::setprecision(9)
            << "影像和GNSS匹配成功" 
            <<" 經度: "<<latitude
            <<" 緯度: "<<longitude
            <<" 高度: "<<altitude
            <<std::endl;

            // 送入 位姿估計執行緒

            cv::imshow("Matched Image", cv_image->image);
            cv::waitKey(1);
            break;

          
        }
        else if(gps_timestamp < image_timestamp - 0.01)// GPS先開始採集，影像滯後，比影像早期的GPS丟棄
        {
             gpsQueue.pop();
             //break;
        }
        else if(gps_timestamp > image_timestamp + 0.01)// GPS採集快，影像處理慢，比影像快的GPS保留，等待後續影像來匹配
        {
            break;
        }

    }


    gpsQueue_Lock.unlock();// gps
}



int main(int argc, char** argv) {
    // Initialize ROS
    ros::init(argc, argv, "image_gps_matcher");
    ros::NodeHandle nh;

    // Subscribe to image and GPS topics
    ros::Subscriber image_sub = nh.subscribe("/camera/image_raw", 10, imageCallback);
    ros::Subscriber gps_sub = nh.subscribe("/gps/fix", 10, gpsCallback);
    cv::namedWindow("Matched Image", cv::WINDOW_AUTOSIZE);

    // 使用 MultiThreadedSpinner，指定執行緒數為 4
    //ros::MultiThreadedSpinner spinner(4);
    // 呼叫 spinner 來處理回撥函式
    //spinner.spin();


    /*
        阻塞呼叫，處理回撥函式
        ros::spin() 使節點進入一個迴圈，不斷處理所有回撥函式，直到節點被關閉或中斷。
        適用於那些只依賴於回撥函式的節點，例如只處理訂閱訊息和服務請求的節點。
        它會阻塞當前執行緒，使得節點在處理回撥函式時不會執行其他程式碼。
        如果你的節點沒有其他需要執行的任務，使用 ros::spin() 可以保持節點活躍，確保所有回撥函式得到處理。
    */
    ros::spin();
     /*
        非阻塞呼叫 
        處理所有到達的回撥函式一次，然後返回。
        它是非阻塞的，允許主迴圈繼續執行其他程式碼。
        當你需要在節點中執行除回撥處理之外的其他邏輯時，例如定時任務、計算或其他非 ROS 相關的操作。
        你可以在主迴圈中呼叫 ros::spinOnce()，使其在處理回撥函式的同時執行其他任務。
     */
    
    //ros::spinOnce();

    return 0;
}

ros(1-2) 影像和GPS釋出節點python版本
2024-08-23
ROSPython
ros節點名稱空間
2018-05-18
ROS
Ros2功能包組織C++節點【RM演算法組】
2024-11-27
ROSC++演算法
ROS2學習之旅（15）——編寫簡單的服務和客戶節點（C++）
2021-07-15
ROSC++
ROS2學習之旅（21）——建立一個動作服務和客戶節點（C++）
2021-07-16
ROSC++
ROS2學習之旅（14）——編寫簡單的釋出者和訂閱者（C++）
2021-07-14
ROSC++
Kubernetes 1.14 版本釋出：正式支援Windows 節點，持久化本地捲進入GA
2019-03-26
Windows持久化
思嵐科技釋出SLAMWARE ROS SDK，全面支援ROS系統
2019-08-22
SLAMROS
【cartographer_ros】五：釋出和訂閱陀螺儀Imu資訊
2022-07-08
ROS
【cartographer_ros】六：釋出和訂閱路標landmark資訊
2022-07-11
ROS
Fescar 釋出 0.2.3 版本，支援 Redis 和 Apollo
2019-03-19
Redis
go-carbon 1.4.2 版本釋出，新增年代和季節系列方法以及修復bug
2021-07-14
Go
RDS MySQL 5.7三節點企業版重磅釋出
2019-10-22
MySql
jQuery 釋出 3.4.0 版本
2019-05-05
jQuery
Rust 1.83.0 版本釋出
2024-11-29
Rust
從零開始學習C++（1-1）
2024-08-21
C++
GeoServer釋出影像WMTS服務
2023-10-09
Server
除夕快樂，Hyperf 釋出 NSQ 元件和 1.1.17 版本
2020-01-24
元件
C++(1) gps轉換為enu
2024-07-15
C++
SOA新版本釋出
2024-04-18
openGauss 2.0.0 版本正式釋出
2021-04-12
[版本釋出]ThinkOX V1正式版釋出【版本號V1.0.2】
2019-05-11
Shotwell 0.32.0影像瀏覽器釋出
2023-05-06
瀏覽器
Apache Doris 1.2.4 Release 版本正式釋出｜版本通告
2023-04-27
Apache
c++ -- &和 * 的解釋
2020-12-06
C++
Solon 1.6.6 釋出，細節打磨
2021-12-14
優麒麟 14.04.6 LTS 版本釋出！
2019-03-08
Laravel-admin 釋出 1.6.10 版本
2019-03-11
Laravel
Apache Doris 2.0.5 版本正式釋出
2024-02-28
Apache
Apache Doris 2.0.5 版本正式釋出！
2024-02-29
Apache
Apache Doris 2.0.4 版本正式釋出
2024-01-29
Apache
Apache Doris 2.0.3 版本正式釋出
2023-12-20
Apache
Android Studio 4.1 Canary 版本釋出
2020-03-07
Android
yiigo 3.x 版本釋出
2019-09-02
Go
Swoftcli: 釋出 v0.1.5 版本
2019-12-01
yiigo 4.x 版本釋出
2019-10-26
Go
Laravel-admin 釋出 1.7.3 版本
2019-07-22
Laravel
Kali Linux釋出2020.1a版本
2020-02-18
Linux

ros(1-1) 影像和GPS釋出節點 c++版本

執行指令

相關文章