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 transform_twist_with_covariance_to_global(self, pose_with_covariance, twist_with_covariance): global_twist = transform_local_twist_to_global( pose_with_covariance.pose, twist_with_covariance.twist) global_twist_cov = transform_local_twist_covariance_to_global( pose_with_covariance.pose, twist_with_covariance.covariance) return TwistWithCovariance(global_twist, global_twist_cov)
def calculate_odometry(self, odom, source_odom): result_odom = copy.deepcopy(source_odom) result_odom.header.frame_id = self.odom_frame result_odom.child_frame_id = self.base_link_frame # consider only pose because twist is local and copied from source_odom position = [source_odom.pose.pose.position.x, source_odom.pose.pose.position.y, source_odom.pose.pose.position.z] orientation = [source_odom.pose.pose.orientation.x, source_odom.pose.pose.orientation.y, source_odom.pose.pose.orientation.z, source_odom.pose.pose.orientation.w] # calculate pose (use odometry source) source_homogeneous_matrix = make_homogeneous_matrix(position, orientation) result_homogeneous_matrix = numpy.dot(source_homogeneous_matrix, self.offset_homogeneous_matrix) result_odom.pose.pose.position = Point(*list(result_homogeneous_matrix[:3, 3])) result_odom.pose.pose.orientation = Quaternion(*list(tf.transformations.quaternion_from_matrix(result_homogeneous_matrix))) # calculate pose covariance (do not use odometry source) if self.odom is not None: result_odom.pose.covariance = self.odom.pose.covariance # do not use source_odom covariance in pose dt = (self.source_odom.header.stamp - self.odom.header.stamp).to_sec() else: # initial covariance of pose is defined as same value of source_odom dt = 0.0 global_twist_covariance = transform_local_twist_covariance_to_global(result_odom.pose.pose, result_odom.twist.covariance) result_odom.pose.covariance = update_pose_covariance(result_odom.pose.covariance, global_twist_covariance, dt) self.odom = result_odom
def source_odom_callback(self, msg): with self.lock: if self.offset_matrix != None: source_odom_matrix = make_homogeneous_matrix([getattr(msg.pose.pose.position, attr) for attr in ["x", "y", "z"]], [getattr(msg.pose.pose.orientation, attr) for attr in ["x", "y", "z", "w"]]) new_odom = copy.deepcopy(msg) new_odom.header.frame_id = self.odom_frame new_odom.child_frame_id = self.base_link_frame twist_list = [new_odom.twist.twist.linear.x, new_odom.twist.twist.linear.y, new_odom.twist.twist.linear.z, new_odom.twist.twist.angular.x, new_odom.twist.twist.angular.y, new_odom.twist.twist.angular.z] # use median filter to cancel spike noise of twist when use_twist_filter is true if self.use_twist_filter: twist_list = self.median_filter(twist_list) new_odom.twist.twist = Twist(Vector3(*twist_list[0:3]), Vector3(*twist_list[3: 6])) # overwrite twist covariance when calculate_covariance flag is True if self.overwrite_pdf: if not all([abs(x) < 1e-3 for x in twist_list]): # shift twist according to error mean when moving (stopping state is trusted) twist_list = [x + y for x, y in zip(twist_list, self.v_err_mean)] new_odom.twist.twist = Twist(Vector3(*twist_list[0:3]), Vector3(*twist_list[3: 6])) # calculate twist covariance according to standard diviation if self.twist_proportional_sigma: current_sigma = [x * y for x, y in zip(twist_list, self.v_err_sigma)] else: if all([abs(x) < 1e-3 for x in twist_list]): current_sigma = [1e-6] * 6 # trust stopping state else: current_sigma = self.v_err_sigma new_odom.twist.covariance = update_twist_covariance(new_odom.twist, current_sigma) # offset coords new_odom_matrix = self.offset_matrix.dot(source_odom_matrix) new_odom.pose.pose.position = Point(*list(new_odom_matrix[:3, 3])) new_odom.pose.pose.orientation = Quaternion(*list(tf.transformations.quaternion_from_matrix(new_odom_matrix))) if self.overwrite_pdf: if self.prev_odom != None: dt = (new_odom.header.stamp - self.prev_odom.header.stamp).to_sec() global_twist_with_covariance = TwistWithCovariance(transform_local_twist_to_global(new_odom.pose.pose, new_odom.twist.twist), transform_local_twist_covariance_to_global(new_odom.pose.pose, new_odom.twist.covariance)) new_odom.pose.covariance = update_pose_covariance(self.prev_odom.pose.covariance, global_twist_with_covariance.covariance, dt) else: new_odom.pose.covariance = numpy.diag([0.01**2] * 6).reshape(-1,).tolist() # initial covariance is assumed to be constant else: # only offset pose covariance new_pose_cov_matrix = numpy.matrix(new_odom.pose.covariance).reshape(6, 6) rotation_matrix = self.offset_matrix[:3, :3] new_pose_cov_matrix[:3, :3] = (rotation_matrix.T).dot(new_pose_cov_matrix[:3, :3].dot(rotation_matrix)) new_pose_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot(new_pose_cov_matrix[3:6, 3:6].dot(rotation_matrix)) new_odom.pose.covariance = numpy.array(new_pose_cov_matrix).reshape(-1,).tolist() # publish self.pub.publish(new_odom) if self.publish_tf: broadcast_transform(self.broadcast, new_odom, self.invert_tf) self.prev_odom = new_odom else: self.offset_matrix = self.calculate_offset(msg)
def calculate_odometry(self, odom, source_odom): result_odom = copy.deepcopy(source_odom) result_odom.header.frame_id = self.odom_frame result_odom.child_frame_id = self.base_link_frame # consider only pose because twist is local and copied from source_odom position = [ source_odom.pose.pose.position.x, source_odom.pose.pose.position.y, source_odom.pose.pose.position.z ] orientation = [ source_odom.pose.pose.orientation.x, source_odom.pose.pose.orientation.y, source_odom.pose.pose.orientation.z, source_odom.pose.pose.orientation.w ] # calculate pose (use odometry source) source_homogeneous_matrix = make_homogeneous_matrix( position, orientation) result_homogeneous_matrix = numpy.dot(source_homogeneous_matrix, self.offset_homogeneous_matrix) result_odom.pose.pose.position = Point( *list(result_homogeneous_matrix[:3, 3])) result_odom.pose.pose.orientation = Quaternion(*list( tf.transformations.quaternion_from_matrix( result_homogeneous_matrix))) # calculate pose covariance (do not use odometry source) if self.odom != None: result_odom.pose.covariance = self.odom.pose.covariance # do not use source_odom covariance in pose dt = (self.source_odom.header.stamp - self.odom.header.stamp).to_sec() else: # initial covariance of pose is defined as same value of source_odom dt = 0.0 global_twist_covariance = transform_local_twist_covariance_to_global( result_odom.pose.pose, result_odom.twist.covariance) result_odom.pose.covariance = update_pose_covariance( result_odom.pose.covariance, global_twist_covariance, dt) self.odom = result_odom
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 source_odom_callback(self, msg): with self.lock: if self.offset_matrix is not None: source_odom_matrix = make_homogeneous_matrix([ getattr(msg.pose.pose.position, attr) for attr in ["x", "y", "z"] ], [ getattr(msg.pose.pose.orientation, attr) for attr in ["x", "y", "z", "w"] ]) new_odom = copy.deepcopy(msg) new_odom.header.frame_id = self.odom_frame new_odom.child_frame_id = self.base_link_frame twist_list = [ new_odom.twist.twist.linear.x, new_odom.twist.twist.linear.y, new_odom.twist.twist.linear.z, new_odom.twist.twist.angular.x, new_odom.twist.twist.angular.y, new_odom.twist.twist.angular.z ] # use median filter to cancel spike noise of twist when use_twist_filter is true if self.use_twist_filter: twist_list = self.median_filter(twist_list) new_odom.twist.twist = Twist(Vector3(*twist_list[0:3]), Vector3(*twist_list[3:6])) # overwrite twist covariance when calculate_covariance flag is True if self.overwrite_pdf: if not all([abs(x) < 1e-3 for x in twist_list]): # shift twist according to error mean when moving (stopping state is trusted) twist_list = [ x + y for x, y in zip(twist_list, self.v_err_mean) ] new_odom.twist.twist = Twist(Vector3(*twist_list[0:3]), Vector3(*twist_list[3:6])) # calculate twist covariance according to standard diviation if self.twist_proportional_sigma: current_sigma = [ x * y for x, y in zip(twist_list, self.v_err_sigma) ] else: if all([abs(x) < 1e-3 for x in twist_list]): current_sigma = [1e-6] * 6 # trust stopping state else: current_sigma = self.v_err_sigma new_odom.twist.covariance = update_twist_covariance( new_odom.twist, current_sigma) # offset coords new_odom_matrix = self.offset_matrix.dot(source_odom_matrix) new_odom.pose.pose.position = Point(*list(new_odom_matrix[:3, 3])) new_odom.pose.pose.orientation = Quaternion(*list( tf.transformations.quaternion_from_matrix( new_odom_matrix))) if self.overwrite_pdf: if self.prev_odom is not None: dt = (new_odom.header.stamp - self.prev_odom.header.stamp).to_sec() global_twist_with_covariance = TwistWithCovariance( transform_local_twist_to_global( new_odom.pose.pose, new_odom.twist.twist), transform_local_twist_covariance_to_global( new_odom.pose.pose, new_odom.twist.covariance)) new_odom.pose.covariance = update_pose_covariance( self.prev_odom.pose.covariance, global_twist_with_covariance.covariance, dt) else: new_odom.pose.covariance = numpy.diag( [0.01**2] * 6).reshape(-1, ).tolist( ) # initial covariance is assumed to be constant else: # only offset pose covariance new_pose_cov_matrix = numpy.matrix( new_odom.pose.covariance).reshape(6, 6) rotation_matrix = self.offset_matrix[:3, :3] new_pose_cov_matrix[:3, :3] = (rotation_matrix.T).dot( new_pose_cov_matrix[:3, :3].dot(rotation_matrix)) new_pose_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot( new_pose_cov_matrix[3:6, 3:6].dot(rotation_matrix)) new_odom.pose.covariance = numpy.array( new_pose_cov_matrix).reshape(-1, ).tolist() # publish self.pub.publish(new_odom) if self.publish_tf: broadcast_transform(self.broadcast, new_odom, self.invert_tf) self.prev_odom = new_odom else: self.offset_matrix = self.calculate_offset(msg)
def transform_twist_with_covariance_to_global(self, pose_with_covariance, twist_with_covariance): global_twist = transform_local_twist_to_global(pose_with_covariance.pose, twist_with_covariance.twist) global_twist_cov = transform_local_twist_covariance_to_global(pose_with_covariance.pose, twist_with_covariance.covariance) return TwistWithCovariance(global_twist, global_twist_cov)