Merge pull request #275 from FrederikPlahl/fix/odom_laser

Fix emulation bug in transform odom -> base_footprint

Author: fmessmer

cob_hardware_emulation/scripts/emulation_nav.py

@@ -51,7 +51,7 @@ def __init__(self, odom_frame):
         quat = tf.transformations.quaternion_from_euler(0, 0, initialpose[2])
         self._odom_transform.rotation = Quaternion(*quat)
 
-        rospy.Timer(rospy.Duration(0.1), self.timer_cb)
+        rospy.Timer(rospy.Duration(0.04), self.timer_cb)
 
         rospy.loginfo("Emulation for navigation running")
 

cob_hardware_emulation/scripts/emulation_odom_laser.py

@@ -1,12 +1,9 @@
 #!/usr/bin/env python
 
 import argparse
-import copy
-import math
 import rospy
-import tf
 import tf2_ros
-from geometry_msgs.msg import Twist, TransformStamped
+from geometry_msgs.msg import TransformStamped
 from nav_msgs.msg import Odometry
 
 class EmulationOdomLaser(object):
@@ -31,55 +28,24 @@ def __init__(self, odom_frame):
 
         self._transform_broadcaster = tf2_ros.TransformBroadcaster()
 
-        self._timestamp_last_update = rospy.Time.now()
-        self._timestamp_last_odom = rospy.Time(0)
-
         self._odom = Odometry()
         self._odom.header.frame_id = self._odom_frame
         self._odom.child_frame_id = "base_footprint"
         self._odom.pose.pose.orientation.w = 1 # initialize orientation with a valid quaternion
 
-        rospy.Timer(rospy.Duration(0.1), self.timer_cb)
-
         rospy.loginfo("Emulation for laser odometry running")
 
     def odometry_callback(self, msg):
         self._odom = msg
         self._odom.header.frame_id = self._odom_frame
         self._odom_publisher.publish(self._odom)
+        self.publish_tf()
 
-    def timer_cb(self, event):
-        # move robot (calculate new pose)
-        dt = rospy.Time.now() - self._timestamp_last_update
-        self._timestamp_last_update = rospy.Time.now()
-        time_since_last_odom = rospy.Time.now() - self._timestamp_last_odom
-
-        #print "dt =", dt.to_sec(), ". duration since last twist =", time_since_last_odom.to_sec()
-        # we assume we're not moving any more if there is no new twist after 0.1 sec
-        if time_since_last_odom < rospy.Duration(0.1):
-            new_pose = copy.deepcopy(self._odom.pose.pose)
-            yaw = tf.transformations.euler_from_quaternion([self._odom.pose.pose.orientation.x, self._odom.pose.pose.orientation.y, self._odom.pose.pose.orientation.z, self._odom.pose.pose.orientation.w])[2] + self._odom.twist.twist.angular.z * dt.to_sec()
-            quat = tf.transformations.quaternion_from_euler(0, 0, yaw)
-            new_pose.orientation.x = quat[0]
-            new_pose.orientation.y = quat[1]
-            new_pose.orientation.z = quat[2]
-            new_pose.orientation.w = quat[3]
-            new_pose.position.x += self._odom.twist.twist.linear.x * dt.to_sec() * math.cos(yaw) - self._odom.twist.twist.linear.y * dt.to_sec() * math.sin(yaw)
-            new_pose.position.y += self._odom.twist.twist.linear.x * dt.to_sec() * math.sin(yaw) + self._odom.twist.twist.linear.y * dt.to_sec() * math.cos(yaw)
-            self._odom.pose.pose = new_pose
-
-            # we're moving, so we set a non-zero twist
-            self._odom.twist.twist.linear.x = self._odom.twist.twist.linear.x
-            self._odom.twist.twist.linear.y = self._odom.twist.twist.linear.y
-            self._odom.twist.twist.angular.z = self._odom.twist.twist.angular.z
-        else:
-            # reset twist as we're not moving anymore
-            self._odom.twist.twist = Twist()
-
+    def publish_tf(self):
         # publish tf
         # pub base_footprint --> odom_frame
         t_odom = TransformStamped()
-        t_odom.header.stamp = rospy.Time.now()
+        t_odom.header.stamp = self._odom.header.stamp
         t_odom.header.frame_id = self._odom_frame
         t_odom.child_frame_id = "base_footprint"
         t_odom.transform.translation = self._odom.pose.pose.position