Added gazebo_ground_truth.py. Odom: fixed ground truth init when imu is also used.

CMakeLists.txt

@@ -576,6 +576,7 @@ catkin_install_python(PROGRAMS
   scripts/yaml_to_camera_info.py
   scripts/netvlad_tf_ros.py
   scripts/wifi_signal_pub.py
+  scripts/gazebo_ground_truth.py
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 

scripts/gazebo_ground_truth.py

@@ -0,0 +1,54 @@
+#!/usr/bin/env python
+import rospy
+import tf
+
+from tf2_msgs.msg import TFMessage
+from gazebo_msgs.msg import LinkStates
+from geometry_msgs.msg import TransformStamped
+
+target_frame_id = ""
+
+def callBack(linkStates):
+	global delta, first
+
+	found = False
+	for i in range(len(linkStates.name)):
+		if linkStates.name[i] == gazebo_frame_id:
+			p = linkStates.pose[i]
+			found = True
+			break
+	
+	if not found:
+ 	    roslog.warn("Gazebo link state \"" + gazebo_frame_id +"\" not found, cannot generate ground truth.")
+	    return
+   
+	t = TransformStamped()
+	t.header.frame_id = frame_id
+        t.header.stamp = rospy.Time.now()
+
+	t.child_frame_id = child_frame_id
+
+	t.transform.translation.x = p.position.x
+	t.transform.translation.y = p.position.y
+	t.transform.translation.z = p.position.z 
+
+	t.transform.rotation.x = p.orientation.x
+	t.transform.rotation.y = p.orientation.y
+	t.transform.rotation.z = p.orientation.z
+	t.transform.rotation.w = p.orientation.w
+	
+	tf_pub.publish(TFMessage([t]))
+
+if __name__ == '__main__':
+    rospy.init_node('generate_gazebo_ground_truth', disable_signals=True)
+
+    frame_id = rospy.get_param('~frame_id', 'world')
+    child_frame_id = rospy.get_param('~child_frame_id', 'base_link_gt')
+    gazebo_frame_id = rospy.get_param('~gazebo_frame_id', 'base_link')
+
+    gazebo_sub = rospy.Subscriber('/gazebo/link_states', LinkStates, callBack)
+   
+    tf_pub = rospy.Publisher('/tf', TFMessage, queue_size=10)
+    tf.TransformBroadcaster()
+
+    rospy.spin()

src/OdometryROS.cpp

@@ -538,7 +538,11 @@ void OdometryROS::processData(SensorData & data, const std_msgs::Header & header
 
 			if(!data.imageRaw().empty() || !data.laserScanRaw().isEmpty())
 			{
-				if(odometry_->getPose().isIdentity())
+				// Use only XYZ to handle the case odometry was previously initialized with IMU,
+				// we assume that the ground truth contains also a real initial orientation
+				float x,y,z;
+				odometry_->getPose().getTranslation(x, y, z);
+				if(x==0.0f && y==0.0f && z==0.0f)
 				{
 					// sync with the first value of the ground truth
 					if(groundTruth.isNull())