Merge pull request UZ-SLAMLab#64 from ccamposm/master

Update ROS scripts

Author: jj-gomez

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();
 

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();
 }

Examples/ROS/ORB_SLAM3/src/ros_stereo.cc

@@ -107,8 +107,8 @@ int main(int argc, char **argv)
 
     ros::NodeHandle nh;
 
-    message_filters::Subscriber<sensor_msgs::Image> left_sub(nh, "/camera/left/image_raw", 100);
-    message_filters::Subscriber<sensor_msgs::Image> right_sub(nh, "/camera/right/image_raw", 100);
+    message_filters::Subscriber<sensor_msgs::Image> left_sub(nh, "/camera/left/image_raw", 1);
+    message_filters::Subscriber<sensor_msgs::Image> right_sub(nh, "/camera/right/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), left_sub,right_sub);
     sync.registerCallback(boost::bind(&ImageGrabber::GrabStereo,&igb,_1,_2));

Examples/ROS/ORB_SLAM3/src/ros_stereo_inertial.cc

@@ -154,13 +154,17 @@ 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();
 }
 
 void ImageGrabber::GrabImageRight(const sensor_msgs::ImageConstPtr &img_msg)
 {
   mBufMutexRight.lock();
+  if (!imgRightBuf.empty())
+    imgRightBuf.pop();
   imgRightBuf.push(img_msg);
   mBufMutexRight.unlock();
 }