def feedback_odom_callback(self, msg): if not self.odom: return self.feedback_odom = msg with self.lock: # check distribution accuracy nearest_odom = copy.deepcopy(self.odom) nearest_dt = (self.feedback_odom.header.stamp - self.odom.header.stamp).to_sec() for hist in self.odom_history: # get neaerest odom from feedback_odom referencing timestamp dt = (self.feedback_odom.header.stamp - hist.header.stamp).to_sec() if abs(dt) < abs(nearest_dt): nearest_dt = dt nearest_odom = copy.deepcopy(hist) # get approximate pose at feedback_odom timestamp (probably it is past) of nearest_odom global_nearest_odom_twist = transform_local_twist_to_global(nearest_odom.pose.pose, nearest_odom.twist.twist) nearest_odom.pose.pose = update_pose(nearest_odom.pose.pose, global_nearest_odom_twist, nearest_dt) global_nearest_odom_twist_covariance = transform_local_twist_covariance_to_global(nearest_odom.pose.pose, nearest_odom.twist.covariance) nearest_odom.pose.covariance = update_pose_covariance(nearest_odom.pose.covariance, global_nearest_odom_twist_covariance, nearest_dt) enable_feedback = self.check_covariance(nearest_odom) or self.check_distribution_difference(nearest_odom, self.feedback_odom) or self.check_feedback_time() # update feedback_odom to approximate odom at current timestamp using previsous velocities if enable_feedback: rospy.loginfo("%s: Feedback enabled.", rospy.get_name()) # adjust timestamp of self.feedback_odom to current self.odom for hist in self.odom_history: dt = (hist.header.stamp - self.feedback_odom.header.stamp).to_sec() if dt > 0.0: # update pose and twist according to the history self.update_twist(self.feedback_odom.twist, hist.twist) global_hist_twist = transform_local_twist_to_global(hist.pose.pose, hist.twist.twist) self.feedback_odom.pose.pose = update_pose(self.feedback_odom.pose.pose, global_hist_twist, dt) # update feedback_odom according to twist of hist # update covariance self.feedback_odom.twist.covariance = hist.twist.covariance global_hist_twist_covariance = transform_local_twist_covariance_to_global(self.feedback_odom.pose.pose, hist.twist.covariance) self.feedback_odom.pose.covariance = update_pose_covariance(self.feedback_odom.pose.covariance, global_hist_twist_covariance, dt) self.feedback_odom.header.stamp = hist.header.stamp dt = (self.odom.header.stamp - self.feedback_odom.header.stamp).to_sec() global_feedback_odom_twist = transform_local_twist_to_global(self.feedback_odom.pose.pose, self.feedback_odom.twist.twist) self.feedback_odom.pose.pose = update_pose(self.feedback_odom.pose.pose, global_feedback_odom_twist, dt) global_feedback_odom_twist_covariance = transform_local_twist_covariance_to_global(self.feedback_odom.pose.pose, self.feedback_odom.twist.covariance) self.feedback_odom.pose.covariance = update_pose_covariance(self.feedback_odom.pose.covariance, global_feedback_odom_twist_covariance, dt) self.feedback_odom.header.stamp = self.odom.header.stamp # integrate feedback_odom and current odom new_odom_pose, new_odom_cov = self.calculate_mean_and_covariance(self.odom.pose, self.feedback_odom.pose) # update self.odom according to the result of integration quat = tf.transformations.quaternion_from_euler(*new_odom_pose[3:6]) self.odom.pose.pose = Pose(Point(*new_odom_pose[0:3]), Quaternion(*quat)) self.odom.pose.covariance = new_odom_cov self.prev_feedback_time = self.odom.header.stamp self.odom_history = [] # update offset new_pose_homogeneous_matrix = make_homogeneous_matrix([self.odom.pose.pose.position.x, self.odom.pose.pose.position.y, self.odom.pose.pose.position.z], [self.odom.pose.pose.orientation.x, self.odom.pose.pose.orientation.y, self.odom.pose.pose.orientation.z, self.odom.pose.pose.orientation.w]) source_homogeneous_matrix = make_homogeneous_matrix([self.source_odom.pose.pose.position.x, self.source_odom.pose.pose.position.y, self.source_odom.pose.pose.position.z], [self.source_odom.pose.pose.orientation.x, self.source_odom.pose.pose.orientation.y, self.source_odom.pose.pose.orientation.z, self.source_odom.pose.pose.orientation.w]) # Hnew = Hold * T -> T = Hold^-1 * Hnew self.offset_homogeneous_matrix = numpy.dot(numpy.linalg.inv(source_homogeneous_matrix), new_pose_homogeneous_matrix) # self.odom.header.stamp is assumed to be same as self.source_odom.header.stamp
def update_pose_with_covariance(self, pose_with_covariance, twist_with_covariance, dt): global_twist_with_covariance = self.transform_twist_with_covariance_to_global( pose_with_covariance, twist_with_covariance) ret_pose = update_pose(pose_with_covariance.pose, global_twist_with_covariance.twist, dt) ret_pose_cov = update_pose_covariance( pose_with_covariance.covariance, global_twist_with_covariance.covariance, dt) return PoseWithCovariance(ret_pose, ret_pose_cov)
def sampling(self, particles, source_odom): global_twist_with_covariance = self.transform_twist_with_covariance_to_global( source_odom.pose, source_odom.twist) sampled_velocities = self.state_transition_probability_rvs( global_twist_with_covariance.twist, global_twist_with_covariance.covariance ) # make sampeld velocity at once because multivariate_normal calculates invert matrix and it is slow dt = (source_odom.header.stamp - self.odom.header.stamp).to_sec() return [ update_pose(prt, Twist(Vector3(*vel[0:3]), Vector3(*vel[3:6])), dt) for prt, vel in zip(particles, sampled_velocities) ]
def feedback_odom_callback(self, msg): if not self.odom: return self.feedback_odom = msg with self.lock: # check distribution accuracy nearest_odom = copy.deepcopy(self.odom) nearest_dt = (self.feedback_odom.header.stamp - self.odom.header.stamp).to_sec() for hist in self.odom_history: # get neaerest odom from feedback_odom referencing timestamp dt = (self.feedback_odom.header.stamp - hist.header.stamp).to_sec() if abs(dt) < abs(nearest_dt): nearest_dt = dt nearest_odom = copy.deepcopy(hist) # get approximate pose at feedback_odom timestamp (probably it is past) of nearest_odom global_nearest_odom_twist = transform_local_twist_to_global( nearest_odom.pose.pose, nearest_odom.twist.twist) nearest_odom.pose.pose = update_pose(nearest_odom.pose.pose, global_nearest_odom_twist, nearest_dt) global_nearest_odom_twist_covariance = transform_local_twist_covariance_to_global( nearest_odom.pose.pose, nearest_odom.twist.covariance) nearest_odom.pose.covariance = update_pose_covariance( nearest_odom.pose.covariance, global_nearest_odom_twist_covariance, nearest_dt) enable_feedback = self.check_covariance( nearest_odom) or self.check_distribution_difference( nearest_odom, self.feedback_odom) or self.check_feedback_time() # update feedback_odom to approximate odom at current timestamp using previsous velocities if enable_feedback: rospy.loginfo("%s: Feedback enabled.", rospy.get_name()) # adjust timestamp of self.feedback_odom to current self.odom for hist in self.odom_history: dt = (hist.header.stamp - self.feedback_odom.header.stamp).to_sec() if dt > 0.0: # update pose and twist according to the history self.update_twist(self.feedback_odom.twist, hist.twist) global_hist_twist = transform_local_twist_to_global( hist.pose.pose, hist.twist.twist) self.feedback_odom.pose.pose = update_pose( self.feedback_odom.pose.pose, global_hist_twist, dt ) # update feedback_odom according to twist of hist # update covariance self.feedback_odom.twist.covariance = hist.twist.covariance global_hist_twist_covariance = transform_local_twist_covariance_to_global( self.feedback_odom.pose.pose, hist.twist.covariance) self.feedback_odom.pose.covariance = update_pose_covariance( self.feedback_odom.pose.covariance, global_hist_twist_covariance, dt) self.feedback_odom.header.stamp = hist.header.stamp dt = (self.odom.header.stamp - self.feedback_odom.header.stamp).to_sec() global_feedback_odom_twist = transform_local_twist_to_global( self.feedback_odom.pose.pose, self.feedback_odom.twist.twist) self.feedback_odom.pose.pose = update_pose( self.feedback_odom.pose.pose, global_feedback_odom_twist, dt) global_feedback_odom_twist_covariance = transform_local_twist_covariance_to_global( self.feedback_odom.pose.pose, self.feedback_odom.twist.covariance) self.feedback_odom.pose.covariance = update_pose_covariance( self.feedback_odom.pose.covariance, global_feedback_odom_twist_covariance, dt) self.feedback_odom.header.stamp = self.odom.header.stamp # integrate feedback_odom and current odom new_odom_pose, new_odom_cov = self.calculate_mean_and_covariance( self.odom.pose, self.feedback_odom.pose) # update self.odom according to the result of integration quat = tf.transformations.quaternion_from_euler( *new_odom_pose[3:6]) self.odom.pose.pose = Pose(Point(*new_odom_pose[0:3]), Quaternion(*quat)) self.odom.pose.covariance = new_odom_cov self.prev_feedback_time = self.odom.header.stamp self.odom_history = [] # update offset new_pose_homogeneous_matrix = make_homogeneous_matrix([ self.odom.pose.pose.position.x, self.odom.pose.pose.position.y, self.odom.pose.pose.position.z ], [ self.odom.pose.pose.orientation.x, self.odom.pose.pose.orientation.y, self.odom.pose.pose.orientation.z, self.odom.pose.pose.orientation.w ]) source_homogeneous_matrix = make_homogeneous_matrix([ self.source_odom.pose.pose.position.x, self.source_odom.pose.pose.position.y, self.source_odom.pose.pose.position.z ], [ self.source_odom.pose.pose.orientation.x, self.source_odom.pose.pose.orientation.y, self.source_odom.pose.pose.orientation.z, self.source_odom.pose.pose.orientation.w ]) # Hnew = Hold * T -> T = Hold^-1 * Hnew self.offset_homogeneous_matrix = numpy.dot( numpy.linalg.inv(source_homogeneous_matrix), new_pose_homogeneous_matrix ) # self.odom.header.stamp is assumed to be same as self.source_odom.header.stamp
def sampling(self, particles, source_odom): global_twist_with_covariance = self.transform_twist_with_covariance_to_global(source_odom.pose, source_odom.twist) sampled_velocities = self.state_transition_probability_rvs(global_twist_with_covariance.twist, global_twist_with_covariance.covariance) # make sampeld velocity at once because multivariate_normal calculates invert matrix and it is slow dt = (source_odom.header.stamp - self.odom.header.stamp).to_sec() return [update_pose(prt, Twist(Vector3(*vel[0:3]), Vector3(*vel[3:6])), dt) for prt, vel in zip(particles, sampled_velocities)]
def update_pose_with_covariance(self, pose_with_covariance, twist_with_covariance, dt): global_twist_with_covariance = self.transform_twist_with_covariance_to_global(pose_with_covariance, twist_with_covariance) ret_pose = update_pose(pose_with_covariance.pose, global_twist_with_covariance.twist, dt) ret_pose_cov = update_pose_covariance(pose_with_covariance.covariance, global_twist_with_covariance.covariance, dt) return PoseWithCovariance(ret_pose, ret_pose_cov)