issue: run the camera in real time

[525753936]

Has anyone successfully called ROS to run the camera in real time? I modified a ros_rgbd file to run astra camera in real time, it turned out that the semantic segementation is working but the camera pose cannot be estimated and visualized, and it has been fixed in the same place. Does anyone know why? Here are my codes:

#include "Semantic.h"
#include "System.h"
#include "Converter.h"
#include
#include
#include
#include
#include <opencv2/core/core.hpp>
#include <ros/ros.h>
#include <cv_bridge/cv_bridge.h>
#include <message_filters/subscriber.h>
#include <message_filters/time_synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/PointCloud2.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>
using namespace std;
using namespace ORB_SLAM3;

class ImageGrabber
{
public:
ImageGrabber(ORB_SLAM3::System* pSLAM):mpSLAM(pSLAM){lastStamp = ros::Time::now(); frameCounter = 0;}

void GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD);

ORB_SLAM3::System* mpSLAM;

private:
ros::Time lastStamp;
int frameCounter;
};

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

if(argc != 3)
{
    cerr << endl << "Usage: rosrun ORB_SLAM3 RGBD path_to_vocabulary path_to_settings" << endl;        
    ros::shutdown();
    return 1;
}    
std::string cnn_method = "maskrcnn"; // maskrcnn
float init_delay = 0.05; //usec
int init_frames = 2;    //usec
// delay for each frame

Semantic::GetInstance()->SetSemanticMethod(cnn_method);

// 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::RGBD,true);
Semantic::GetInstance()->Run();
ImageGrabber igb(&SLAM);

ros::NodeHandle nh;

message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/rgb/image_raw", 1);
message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "camera/depth_registered/image_raw", 1);
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;
message_filters::Synchronizer<sync_pol> sync(sync_pol(10), rgb_sub,depth_sub);
sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2));

ros::spin();

// Stop all threads
SLAM.Shutdown();

// Save camera trajectory
SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");

ros::shutdown();

return 0;

}

void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD)
{
// Copy the ros image message to cv::Mat.
cv_bridge::CvImageConstPtr cv_ptrRGB;
try
{
cv_ptrRGB = cv_bridge::toCvShare(msgRGB);
}
catch (cv_bridge::Exception& e)
{
ROS_ERROR("cv_bridge exception: %s", e.what());
return;
}

cv_bridge::CvImageConstPtr cv_ptrD;
try
{
    cv_ptrD = cv_bridge::toCvShare(msgD);
}
catch (cv_bridge::Exception& e)
{
    ROS_ERROR("cv_bridge exception: %s", e.what());
    return;
}

float frame_delay = 0;
cv::Mat Tcw = mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec());

usleep(frame_delay);

frameCounter++;
// Print frame rate every x second
if((cv_ptrRGB->header.stamp - lastStamp).toSec() >= 5.0)
{
    float fps = frameCounter / (cv_ptrRGB->header.stamp - lastStamp).toSec();
    lastStamp = cv_ptrRGB->header.stamp;
    ROS_INFO("Frames per second: %f", fps);
    frameCounter = 0;
}

if(Tcw.empty())
    return;
cv::Mat tcw = Tcw.rowRange(0,3).col(3);

// Publish tf transform
static tf2_ros::TransformBroadcaster br;
geometry_msgs::TransformStamped transformStamped;
transformStamped.header.stamp = cv_ptrRGB->header.stamp;
transformStamped.header.frame_id = "camera_rgb_optical_frame"; 
transformStamped.child_frame_id = "world";
transformStamped.transform.translation.x = tcw.at<float>(0);
transformStamped.transform.translation.y = tcw.at<float>(1);
transformStamped.transform.translation.z = tcw.at<float>(2);
vector<float> q = ORB_SLAM3::Converter::toQuaternion(Tcw.rowRange(0,3).colRange(0,3));
transformStamped.transform.rotation.x = q[0];
transformStamped.transform.rotation.y = q[1];
transformStamped.transform.rotation.z = q[2];
transformStamped.transform.rotation.w = q[3];

br.sendTransform(transformStamped);

}