From 6fa2efd4a8dfb876c4a7c62ac2db4d31fb015006 Mon Sep 17 00:00:00 2001 From: ccamposm <630549@unizar.es> Date: Mon, 17 Aug 2020 09:45:47 +0200 Subject: [PATCH] Update ROS scripts --- Examples/ROS/ORB_SLAM3/src/ros_mono.cc | 2 +- Examples/ROS/ORB_SLAM3/src/ros_mono_inertial.cc | 4 ++-- Examples/ROS/ORB_SLAM3/src/ros_stereo.cc | 4 ++-- Examples/ROS/ORB_SLAM3/src/ros_stereo_inertial.cc | 4 ++++ 4 files changed, 9 insertions(+), 5 deletions(-) diff --git a/Examples/ROS/ORB_SLAM3/src/ros_mono.cc b/Examples/ROS/ORB_SLAM3/src/ros_mono.cc index 2cdfde0a14..08cb014578 100644 --- a/Examples/ROS/ORB_SLAM3/src/ros_mono.cc +++ b/Examples/ROS/ORB_SLAM3/src/ros_mono.cc @@ -59,7 +59,7 @@ int main(int argc, char **argv) ImageGrabber igb(&SLAM); ros::NodeHandle nodeHandler; - ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 100, &ImageGrabber::GrabImage,&igb); + ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage,&igb); ros::spin(); diff --git a/Examples/ROS/ORB_SLAM3/src/ros_mono_inertial.cc b/Examples/ROS/ORB_SLAM3/src/ros_mono_inertial.cc index 85d02925ed..32f592088e 100644 --- a/Examples/ROS/ORB_SLAM3/src/ros_mono_inertial.cc +++ b/Examples/ROS/ORB_SLAM3/src/ros_mono_inertial.cc @@ -105,11 +105,11 @@ int main(int argc, char **argv) return 0; } - - void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr &img_msg) { mBufMutex.lock(); + if (!img0Buf.empty()) + img0Buf.pop(); img0Buf.push(img_msg); mBufMutex.unlock(); } diff --git a/Examples/ROS/ORB_SLAM3/src/ros_stereo.cc b/Examples/ROS/ORB_SLAM3/src/ros_stereo.cc index 574dc0a8c4..3cbcf71792 100644 --- a/Examples/ROS/ORB_SLAM3/src/ros_stereo.cc +++ b/Examples/ROS/ORB_SLAM3/src/ros_stereo.cc @@ -107,8 +107,8 @@ int main(int argc, char **argv) ros::NodeHandle nh; - message_filters::Subscriber left_sub(nh, "/camera/left/image_raw", 100); - message_filters::Subscriber right_sub(nh, "/camera/right/image_raw", 100); + message_filters::Subscriber left_sub(nh, "/camera/left/image_raw", 1); + message_filters::Subscriber right_sub(nh, "/camera/right/image_raw", 1); typedef message_filters::sync_policies::ApproximateTime sync_pol; message_filters::Synchronizer sync(sync_pol(10), left_sub,right_sub); sync.registerCallback(boost::bind(&ImageGrabber::GrabStereo,&igb,_1,_2)); diff --git a/Examples/ROS/ORB_SLAM3/src/ros_stereo_inertial.cc b/Examples/ROS/ORB_SLAM3/src/ros_stereo_inertial.cc index 07cd68c715..d80464cddd 100644 --- a/Examples/ROS/ORB_SLAM3/src/ros_stereo_inertial.cc +++ b/Examples/ROS/ORB_SLAM3/src/ros_stereo_inertial.cc @@ -154,6 +154,8 @@ int main(int argc, char **argv) void ImageGrabber::GrabImageLeft(const sensor_msgs::ImageConstPtr &img_msg) { mBufMutexLeft.lock(); + if (!imgLeftBuf.empty()) + imgLeftBuf.pop(); imgLeftBuf.push(img_msg); mBufMutexLeft.unlock(); } @@ -161,6 +163,8 @@ void ImageGrabber::GrabImageLeft(const sensor_msgs::ImageConstPtr &img_msg) void ImageGrabber::GrabImageRight(const sensor_msgs::ImageConstPtr &img_msg) { mBufMutexRight.lock(); + if (!imgRightBuf.empty()) + imgRightBuf.pop(); imgRightBuf.push(img_msg); mBufMutexRight.unlock(); }