def publish_odometry(self): # relfect source_odom information self.odom.header.stamp = self.source_odom.header.stamp self.odom.twist = self.source_odom.twist # use only important particels combined_prt_weight = zip(self.particles, self.weights) selected_prt_weight = zip( *sorted(combined_prt_weight, key=itemgetter(1), reverse=True)[:int(self.valid_particle_num)] ) # [(p0, w0), (p1, w1), ..., (pN, wN)] -> [(sorted_p0, sorted_w0), (sorted_p1, sorted_w1), ..., (sorted_pN', sorted_wN')] -> [(sorted_p0, ..., sorted_pN'), (sorted_w0, ..., sorted_wN')] # estimate gaussian distribution for Odometry msg mean, cov = self.guess_normal_distribution(selected_prt_weight[0], selected_prt_weight[1]) self.odom.pose.pose = self.convert_list_to_pose(mean) self.odom.pose.covariance = list(itertools.chain(*cov)) self.pub.publish(self.odom) if self.publish_tf: broadcast_transform(self.broadcast, self.odom, self.invert_tf) # update prev_rpy to prevent jump of angles self.prev_rpy = transform_quaternion_to_euler([ self.odom.pose.pose.orientation.x, self.odom.pose.pose.orientation.y, self.odom.pose.pose.orientation.z, self.odom.pose.pose.orientation.w ], self.prev_rpy) # diagnostics self.update_diagnostics(self.particles, self.weights, self.odom.header.stamp)
def convert_pose_to_list(self, pose): euler = transform_quaternion_to_euler( (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w), self.prev_rpy) return [ pose.position.x, pose.position.y, pose.position.z, euler[0], euler[1], euler[2] ]
def publish_odometry(self): # refrect source_odom informations self.pub.publish(self.odom) if self.publish_tf: broadcast_transform(self.broadcast, self.odom, self.invert_tf) # diagnostics self.update_diagnostics(self.particles, self.weights, self.odom.header.stamp) # update prev_rpy to prevent jump of angles self.prev_rpy = transform_quaternion_to_euler([self.odom.pose.pose.orientation.x, self.odom.pose.pose.orientation.y, self.odom.pose.pose.orientation.z, self.odom.pose.pose.orientation.w], self.prev_rpy)
def publish_odometry(self): # relfect source_odom information self.odom.header.stamp = self.source_odom.header.stamp self.odom.twist = self.source_odom.twist # estimate gaussian distribution for Odometry msg mean, cov = self.guess_normal_distribution(self.particles, self.weights) self.odom.pose.pose = self.convert_list_to_pose(mean) self.odom.pose.covariance = list(itertools.chain(*cov)) self.pub.publish(self.odom) if self.publish_tf: broadcast_transform(self.broadcast, self.odom, self.invert_tf) # update prev_rpy to prevent jump of angles self.prev_rpy = transform_quaternion_to_euler([self.odom.pose.pose.orientation.x, self.odom.pose.pose.orientation.y, self.odom.pose.pose.orientation.z, self.odom.pose.pose.orientation.w], self.prev_rpy)
def publish_odometry(self): # relfect source_odom information self.odom.header.stamp = self.source_odom.header.stamp self.odom.twist = self.source_odom.twist # use only important particels combined_prt_weight = zip(self.particles, self.weights) selected_prt_weight = zip(*sorted(combined_prt_weight, key = itemgetter(1), reverse = True)[:int(self.valid_particle_num)]) # [(p0, w0), (p1, w1), ..., (pN, wN)] -> [(sorted_p0, sorted_w0), (sorted_p1, sorted_w1), ..., (sorted_pN', sorted_wN')] -> [(sorted_p0, ..., sorted_pN'), (sorted_w0, ..., sorted_wN')] # estimate gaussian distribution for Odometry msg mean, cov = self.guess_normal_distribution(selected_prt_weight[0], selected_prt_weight[1]) self.odom.pose.pose = self.convert_list_to_pose(mean) self.odom.pose.covariance = list(itertools.chain(*cov)) self.pub.publish(self.odom) if self.publish_tf: broadcast_transform(self.broadcast, self.odom, self.invert_tf) # update prev_rpy to prevent jump of angles self.prev_rpy = transform_quaternion_to_euler([self.odom.pose.pose.orientation.x, self.odom.pose.pose.orientation.y, self.odom.pose.pose.orientation.z, self.odom.pose.pose.orientation.w], self.prev_rpy)
def publish_odometry(self): # relfect source_odom information self.odom.header.stamp = self.source_odom.header.stamp self.odom.twist = self.source_odom.twist # estimate gaussian distribution for Odometry msg mean, cov = self.guess_normal_distribution(self.particles, self.weights) self.odom.pose.pose = self.convert_list_to_pose(mean) self.odom.pose.covariance = list(itertools.chain(*cov)) self.pub.publish(self.odom) if self.publish_tf: broadcast_transform(self.broadcast, self.odom, self.invert_tf) # update prev_rpy to prevent jump of angles self.prev_rpy = transform_quaternion_to_euler([ self.odom.pose.pose.orientation.x, self.odom.pose.pose.orientation.y, self.odom.pose.pose.orientation.z, self.odom.pose.pose.orientation.w ], self.prev_rpy)
def convert_pose_to_list(self, pose): euler = transform_quaternion_to_euler((pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w), self.prev_rpy) return [pose.position.x, pose.position.y, pose.position.z, euler[0], euler[1], euler[2]]