Merge branch 'develop'

Author: david2611

nodes/noisify_odom

@@ -32,7 +32,10 @@ from std_msgs.msg import String
 
 
 def _noisy(std_dev):
-    return std_dev * np.random.randn()
+    # Use the standard deviation as the mean as well as the std deviation
+    # This feels super hacky but for me better represents wheel encoder error
+    # usually perceived distance travelled will be greater than actual distance
+    return np.random.normal(std_dev, std_dev)
 
 
 def _SE3_to_pose_msg(pose):
@@ -70,8 +73,10 @@ class PoseNoisifier(object):
         self._lock = threading.Lock()
 
         self._start_pose = None
+        self._last_time = None
 
-        self._accumulated_error = np.eye(4)
+        self._nodom = np.eye(4)
+        self._initialised = False
         self._last_vel = Twist()
 
         self._tf = tf2_ros.TransformBroadcaster()
@@ -88,25 +93,69 @@ class PoseNoisifier(object):
 
     def pub(self, odom_msg):
         with self._lock:
+
+            current_time = rospy.get_time()
+            time_diff = current_time - self._last_time
             # Bail if we don't have valid data to work with
             if self._start_pose is None:
                 return
 
-            # Translate our clean odom into a noisy odom
-            noisy_odom = np.matmul(self._accumulated_error,
-                                   sp.pose_msg_to_SE3(odom_msg.pose))
+            # Initialise that which needs initialisation
+            if not self._initialised:
+                self._nodom = sp.pose_msg_to_SE3(odom_msg.pose)
+                self._last_odom = sp.pose_msg_to_SE3(odom_msg.pose)
+                self._initialised = True
+            
+            # Calculate the velocity of the last segment of time using odometery.
+            # Note, previous version used command velocity but this differed too much
+            # from "true" velocity
+            # TODO enhancement would be to use actual wheel rotations and translate that into
+            # an estimate of velocity but that is too much for now. Current version thinks
+            # it is stationary even if the wheels keep spinning as the robot hits a wall
+            current_odom = sp.pose_msg_to_SE3(odom_msg.pose)
+            odom_diff = np.matmul(np.linalg.inv(self._last_odom), current_odom)
+            vel_angular = sp.rpy_from_SE3(odom_diff)[2]/time_diff  # angular.z
+            vel_linear = odom_diff[0,3]/time_diff # linear.x
+            
+            # Calculate current noisy pose from starting point (noisy odom pose)
+            # Current velocity with some level of error transformed into an expected pose
+            # Between last and current odom reading
+            # NOTE error is now usually positive multiplier to better simulate wheel encoder
+            # errors (always think you have travelled further than you have)
+            lin_vel_noisy = vel_linear + \
+                            vel_linear * _noisy(self.noise_linear)
+            ang_vel_noisy = vel_angular + \
+                            vel_angular * _noisy(self.noise_angular)
+            
+            # Turn velocities back into distances
+            lin_move = lin_vel_noisy * time_diff
+            ang_move = ang_vel_noisy * time_diff
+            
+            self._nodom = np.matmul(
+                self._nodom,
+                np.matmul(
+                    sp.SE3_from_translation(lin_move),
+                    sp.SE3_from_yaw(ang_move)),
+            )
 
-            # Figure out "localisation" transform from map to odom frame
+            # Calculate the error between what we read here and the true odometry
+            odom_error = np.matmul(sp.pose_msg_to_SE3(odom_msg.pose), 
+                                   np.linalg.inv(self._nodom))
+            
+            # Calculate localisation transform from map to odom frame to accomodate for 
+            # calculated error
             localisation = np.matmul(self._start_pose,
-                                     np.linalg.inv(self._accumulated_error))
+                                     odom_error)
 
             # Publish our messages
             o = Odometry()
             o.header.stamp = odom_msg.header.stamp
             o.header.frame_id = self.out_frame_odom
             o.child_frame_id = self.out_frame_robot
-            o.pose.pose = _SE3_to_pose_msg(noisy_odom)
+            o.pose.pose = _SE3_to_pose_msg(self._nodom)
             o.pose.covariance = odom_msg.pose.covariance
+            # TODO establish whether this twist should be the cmd vel twist 
+            # or calculated "true" velocity
             o.twist.twist = self._last_vel
             self._pub_odom.publish(o)
 
@@ -121,22 +170,19 @@ class PoseNoisifier(object):
             t.header.stamp = odom_msg.header.stamp
             t.header.frame_id = self.out_frame_odom
             t.child_frame_id = self.out_frame_robot
-            t.transform = _SE3_to_tf_msg(noisy_odom)
+            t.transform = _SE3_to_tf_msg(self._nodom)
             self._tf.sendTransform(t)
 
-            # Update accumulated noise
-            self._accumulated_error = np.matmul(
-                self._accumulated_error,
-                np.matmul(
-                    sp.SE3_from_translation(self._last_vel.linear.x *
-                                            _noisy(self.noise_linear)),
-                    sp.SE3_from_yaw(self._last_vel.angular.z *
-                                    _noisy(self.noise_angular))),
-            )
+            # Update time and odom
+            self._last_time = current_time
+            self._last_odom = current_odom
+
+            
 
     def update_start_pose(self, start_pose_msg):
         with self._lock:
-            self._accumulated_error = np.eye(4)
+            self._initialised = False
+            self._last_time = rospy.get_time()
             self._start_pose = sp.wxyzXYZ_to_SE3(*np.array([
                 float(x.strip()) for x in start_pose_msg.data.replace(
                     '[', '').replace(']', '').split(',')

src/robot_callbacks.py

@@ -48,8 +48,9 @@ def _get_noisy_pose(controller, child_frame):
     odom_t_child = sp.tf_msg_to_SE3(
         controller.tf_buffer.lookup_transform('odom', child_frame,
                                               rospy.Time()))
+
     # Noisy child should be init pose + odom_t_child
-    return world_t_init_pose * odom_t_child
+    return np.matmul(world_t_init_pose, odom_t_child)
 
 
 def _current_pose(controller):
@@ -120,7 +121,6 @@ def _move_to_pose(goal, publisher, controller):
         # Get latest position error
         current = sp.SE3_to_SE2(_current_pose(controller))
         error = np.matmul(np.linalg.inv(current), g)
-
         # Calculate values used in the controller
         rho = np.linalg.norm(error[0:2, 2])
         alpha = np.arctan2(error[1, 2], error[0, 2])