def source_odom_callback(self, msg):
with self.lock:
self.source_odom = msg
self.calculate_odometry(self.odom, self.source_odom)
self.publish_odometry()
if self.publish_tf:
broadcast_transform(self.broadcast, self.odom, self.invert_tf)
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 source_odom_callback(self, msg):
with self.lock:
self.source_odom = msg
self.calculate_odometry(self.odom, self.source_odom)
self.publish_odometry()
if self.publish_tf:
broadcast_transform(self.broadcast, self.odom, self.invert_tf)
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 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 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)
