From dc29b741112eb17b458f223e23cdabd339b15208 Mon Sep 17 00:00:00 2001
From: matlabbe <matlabbe@gmail.com>
Date: Sat, 31 Mar 2018 15:44:53 -0400
Subject: [PATCH] OdometryROS: don't publish odom if velocity is null too when
 publish_null_when_lost is false

---
 src/OdometryROS.cpp | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/OdometryROS.cpp b/src/OdometryROS.cpp
index 84d70f53..3843caf5 100644
--- a/src/OdometryROS.cpp
+++ b/src/OdometryROS.cpp
@@ -527,7 +527,10 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 			odom.twist.covariance.at(35) = setTwist?info.reg.covariance.at<double>(5,5):BAD_COVARIANCE; // yawyaw
 
 			//publish the message
-			odomPub_.publish(odom);
+			if(setTwist || publishNullWhenLost_)
+			{
+				odomPub_.publish(odom);
+			}
 		}
 
 		// local map / reference frame