wait a bit after failing compute covariances

src/node.cpp

@@ -101,8 +101,13 @@ void WolfRosNode::solve()
             if (solver_->getVerbosity() != SolverManager::ReportVerbosity::QUIET)
                 ROS_INFO("Covariances computed successfully! It took %li microseconds", duration.count());
         }
-        else if (solver_->getVerbosity() != SolverManager::ReportVerbosity::QUIET)
-            ROS_WARN("Failed to compute covariances");
+        else
+        {
+            // will try again after 10% of cov period
+            last_cov_stamp_ = last_cov_stamp_+ ros::Duration(0.1*cov_period_);
+            if (solver_->getVerbosity() != SolverManager::ReportVerbosity::QUIET)
+                ROS_WARN("Failed to compute covariances");
+        }
     }
 }
 

src/publisher_tf.cpp

@@ -74,7 +74,7 @@ void PublisherTf::publishDerived()
 
     //gets T_map2odom_ (odom wrt map), by using tf listener, and assuming an odometry node is broadcasting odom2base
     tf::StampedTransform T_base2odom;
-    if ( tfl_.waitForTransform(base_frame_id_, odom_frame_id_, ros::Time(0), ros::Duration(0.2)) )
+    if ( tfl_.waitForTransform(base_frame_id_, odom_frame_id_, ros::Time(0), ros::Duration(0.1)) )
     {
         tfl_.lookupTransform(base_frame_id_, odom_frame_id_, ros::Time(0), T_base2odom);
         //std::cout << ros::Time::now().sec << " Odometry: " << T_base2odom.inverse().getOrigin().getX() << " " << T_base2odom.inverse().getOrigin().getY() << " " << T_base2odom.inverse().getRotation().getAngle() << std::endl;