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]]
