執行指令
#=============檔案結構 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; }