.cc文件其实就是复制一份文件，修改它的话题topic
.cc文件
/*** This file is part of ORB-SLAM3** Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.** ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public* License as published by the Free Software Foundation, either version 3 of the License, or* (at your option) any later version.** ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the* GNU General Public License for more details.** You should have received a copy of the GNU General Public License along with ORB-SLAM3.* If not, see .*/#include#include#include#include#include#include#include#include#include#include#include#include#include"../../../include/System.h"#include"include/ImuTypes.h"using namespace std;class ImuGrabber{public:ImuGrabber(){};void GrabImu(const sensor_msgs::ImuConstPtr &imu_msg);queue> imuBuf;std::mutex mBufMutex;};class ImageGrabber{public:ImageGrabber(ORB_SLAM3::System* pSLAM, ImuGrabber *pImuGb, const bool bRect, const bool bClahe): mpSLAM(pSLAM), mpImuGb(pImuGb), do_rectify(bRect), mbClahe(bClahe){}void GrabImageLeft(const sensor_msgs::ImageConstPtr& msg);void GrabImageRight(const sensor_msgs::ImageConstPtr& msg);cv::Mat GetImage(const sensor_msgs::ImageConstPtr &img_msg);void SyncWithImu();queue> imgLeftBuf, imgRightBuf;std::mutex mBufMutexLeft,mBufMutexRight;ORB_SLAM3::System* mpSLAM;ImuGrabber *mpImuGb;const bool do_rectify;cv::Mat M1l,M2l,M1r,M2r;const bool mbClahe;cv::Ptr mClahe = cv::createCLAHE(3.0, cv::Size(8, 8));};int main(int argc, char **argv){ros::init(argc, argv, "Stereo_Inertial");ros::NodeHandle n("~");ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);bool bEqual = false;if(argc < 4 || argc > 5){cerr << endl << "Usage: rosrun ORB_SLAM3 Stereo_Inertial path_to_vocabulary path_to_settings do_rectify [do_equalize]" << endl;ros::shutdown();return 1;}std::string sbRect(argv[3]);if(argc==5){std::string sbEqual(argv[4]);if(sbEqual == "true")bEqual = true;}// Create SLAM system. It initializes all system threads and gets ready to process frames.ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::IMU_STEREO,true);ImuGrabber imugb;ImageGrabber igb(&SLAM,&imugb,sbRect == "true",bEqual);if(igb.do_rectify){// Load settings related to stereo calibrationcv::FileStorage fsSettings(argv[2], cv::FileStorage::READ);if(!fsSettings.isOpened()){cerr << "ERROR: Wrong path to settings" << endl;return -1;}cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r;fsSettings["LEFT.K"] >> K_l;fsSettings["RIGHT.K"] >> K_r;fsSettings["LEFT.P"] >> P_l;fsSettings["RIGHT.P"] >> P_r;fsSettings["LEFT.R"] >> R_l;fsSettings["RIGHT.R"] >> R_r;fsSettings["LEFT.D"] >> D_l;fsSettings["RIGHT.D"] >> D_r;int rows_l = fsSettings["LEFT.height"];int cols_l = fsSettings["LEFT.width"];int rows_r = fsSettings["RIGHT.height"];int cols_r = fsSettings["RIGHT.width"];if(K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() ||rows_l==0 || rows_r==0 || cols_l==0 || cols_r==0){cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl;return -1;}cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,igb.M1l,igb.M2l);cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,igb.M1r,igb.M2r);}// Maximum delay, 5 seconds//ros::Subscriber sub_imu = n.subscribe("/imu", 1000, &ImuGrabber::GrabImu, &imugb);//ros::Subscriber sub_img_left = n.subscribe("/camera/left/image_raw", 100, &ImageGrabber::GrabImageLeft,&igb);//ros::Subscriber sub_img_right = n.subscribe("/camera/right/image_raw", 100, &ImageGrabber::GrabImageRight,&igb);ros::Subscriber sub_imu = n.subscribe("/zed2/zed_node/imu/data_raw", 1000, &ImuGrabber::GrabImu, &imugb);ros::Subscriber sub_img_left = n.subscribe("/zed2/zed_node/left/image_rect_color", 100, &ImageGrabber::GrabImageLeft,&igb);ros::Subscriber sub_img_right = n.subscribe("/zed2/zed_node/right/image_rect_color", 100, &ImageGrabber::GrabImageRight,&igb);std::thread sync_thread(&ImageGrabber::SyncWithImu,&igb);ros::spin();return 0;}void ImageGrabber::GrabImageLeft(const sensor_msgs::ImageConstPtr &img_msg){mBufMutexLeft.lock();if (!imgLeftBuf.empty())imgLeftBuf.pop();imgLeftBuf.push(img_msg);mBufMutexLeft.unlock();}void ImageGrabber::GrabImageRight(const sensor_msgs::ImageConstPtr &img_msg){mBufMutexRight.lock();if (!imgRightBuf.empty())imgRightBuf.pop();imgRightBuf.push(img_msg);mBufMutexRight.unlock();}cv::Mat ImageGrabber::GetImage(const sensor_msgs::ImageConstPtr &img_msg){// Copy the ros image message to cv::Mat.cv_bridge::CvImageConstPtr cv_ptr;try{cv_ptr = cv_bridge::toCvShare(img_msg, sensor_msgs::image_encodings::MONO8);}catch (cv_bridge::Exception& e){ROS_ERROR("cv_bridge exception: %s", e.what());}if(cv_ptr->image.type()==0){return cv_ptr->image.clone();}else{std::cout