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 init_transform_callback(self, msg):
with self.lock:
self.initial_base_link_transform = make_homogeneous_matrix([getattr(msg.transform.translation, attr) for attr in ["x", "y", "z"]], [getattr(msg.transform.rotation, attr) for attr in ["x", "y", "z", "w"]]) # base_odom -> init_odom
self.prev_odom = None
self.offset_matrix = None
if self.use_twist_filter:
self.filter_buffer = []
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_offset(self, source_odom):
# tf relationships: init_odom -> source_odom -> body_link <- init_odom
# offset_odom is init_odom -> source_odom
# TODO: check timestamps of base_odom and source_odom and fix if necessary
if self.initial_base_link_transform == None:
rospy.loginfo("[%s] initial transform is not subscribed yet", rospy.get_name())
return None
source_odom_matrix = make_homogeneous_matrix([getattr(source_odom.pose.pose.position, attr) for attr in ["x", "y", "z"]], [getattr(source_odom.pose.pose.orientation, attr) for attr in ["x", "y", "z", "w"]]) # source_odom -> body_link
return self.initial_base_link_transform.dot(numpy.linalg.inv(source_odom_matrix)) # T(init->src) * T(src->body) = T(init->body)
def base_odom_callback(self, msg):
with self.lock:
self.base_odom = msg
if not self.offset_transform:
# offset is not initialized
base_to_init_transform = make_homogeneous_matrix([0, 0, 0], [0, 0, 0, 1])
self.offset_transform = self.calculate_init_to_base_link_transform(self.base_odom, base_to_init_transform, msg.header.stamp)
if self.offset_transform != None:
self.pub.publish(self.offset_transform)
def __init__(self):
rospy.init_node("OdometryOffset", anonymous=True)
self.offset_matrix = None
self.prev_odom = None
self.initial_base_link_transform = make_homogeneous_matrix(
[0, 0, 0], [0, 0, 0, 1])
self.lock = threading.Lock()
# execute rate
self.rate = float(rospy.get_param("~rate", 100))
self.r = rospy.Rate(self.rate)
# tf parameters
self.publish_tf = rospy.get_param("~publish_tf", True)
if self.publish_tf:
self.broadcast = tf.TransformBroadcaster()
self.invert_tf = rospy.get_param("~invert_tf", True)
self.odom_frame = rospy.get_param("~odom_frame", "offset_odom")
self.base_link_frame = rospy.get_param("~base_link_frame", "BODY")
self.tf_duration = rospy.get_param("~tf_duration", 1)
# for filter (only used when use_twist_filter is True)
self.use_twist_filter = rospy.get_param("~use_twist_filter", False)
if self.use_twist_filter:
self.filter_buffer_size = rospy.get_param("~filter_buffer_size", 5)
self.filter_buffer = []
# to overwrite probability density function (only used when overwrite_pdf is True)
self.overwrite_pdf = rospy.get_param("~overwrite_pdf", False)
if self.overwrite_pdf:
self.twist_proportional_sigma = rospy.get_param(
"~twist_proportional_sigma", False)
self.v_err_mean = [
rospy.get_param("~mean_x", 0.0),
rospy.get_param("~mean_y", 0.0),
rospy.get_param("~mean_z", 0.0),
rospy.get_param("~mean_roll", 0.0),
rospy.get_param("~mean_pitch", 0.0),
rospy.get_param("~mean_yaw", 0.0)
]
self.v_err_sigma = [
rospy.get_param("~sigma_x", 0.05),
rospy.get_param("~sigma_y", 0.1),
rospy.get_param("~sigma_z", 0.0001),
rospy.get_param("~sigma_roll", 0.0001),
rospy.get_param("~sigma_pitch", 0.0001),
rospy.get_param("~sigma_yaw", 0.01)
]
self.reconfigure_server = Server(OdometryOffsetReconfigureConfig,
self.reconfigure_callback)
self.source_odom_sub = rospy.Subscriber("~source_odom", Odometry,
self.source_odom_callback)
self.init_transform_sub = rospy.Subscriber(
"~initial_base_link_transform", TransformStamped,
self.init_transform_callback
) # init_transform is assumed to be transform of init_odom -> base_link
self.pub = rospy.Publisher("~output", Odometry, queue_size=1)
def init_transform_callback(self, msg):
with self.lock:
self.initial_base_link_transform = make_homogeneous_matrix([
getattr(msg.transform.translation, attr)
for attr in ["x", "y", "z"]
], [
getattr(msg.transform.rotation, attr)
for attr in ["x", "y", "z", "w"]
]) # base_odom -> init_odom
self.prev_odom = None
self.offset_matrix = None
if self.use_twist_filter:
self.filter_buffer = []
def __init__(self):
rospy.init_node("OdometryOffset", anonymous=True)
self.offset_matrix = None
self.prev_odom = None
self.initial_base_link_transform = make_homogeneous_matrix([0, 0, 0], [0, 0, 0, 1])
self.lock = threading.Lock()
# execute rate
self.rate = float(rospy.get_param("~rate", 100))
self.r = rospy.Rate(self.rate)
# tf parameters
self.publish_tf = rospy.get_param("~publish_tf", True)
if self.publish_tf:
self.broadcast = tf.TransformBroadcaster()
self.invert_tf = rospy.get_param("~invert_tf", True)
self.odom_frame = rospy.get_param("~odom_frame", "offset_odom")
self.base_link_frame = rospy.get_param("~base_link_frame", "BODY")
self.tf_duration = rospy.get_param("~tf_duration", 1)
# for filter (only used when use_twist_filter is True)
self.use_twist_filter = rospy.get_param("~use_twist_filter", False)
if self.use_twist_filter:
self.filter_buffer_size = rospy.get_param("~filter_buffer_size", 5)
self.filter_buffer = []
# to overwrite probability density function (only used when overwrite_pdf is True)
self.overwrite_pdf = rospy.get_param("~overwrite_pdf", False)
if self.overwrite_pdf:
self.twist_proportional_sigma = rospy.get_param("~twist_proportional_sigma", False)
self.v_err_mean = [
rospy.get_param("~mean_x", 0.0),
rospy.get_param("~mean_y", 0.0),
rospy.get_param("~mean_z", 0.0),
rospy.get_param("~mean_roll", 0.0),
rospy.get_param("~mean_pitch", 0.0),
rospy.get_param("~mean_yaw", 0.0),
]
self.v_err_sigma = [
rospy.get_param("~sigma_x", 0.05),
rospy.get_param("~sigma_y", 0.1),
rospy.get_param("~sigma_z", 0.0001),
rospy.get_param("~sigma_roll", 0.0001),
rospy.get_param("~sigma_pitch", 0.0001),
rospy.get_param("~sigma_yaw", 0.01),
]
self.reconfigure_server = Server(OdometryOffsetReconfigureConfig, self.reconfigure_callback)
self.source_odom_sub = rospy.Subscriber("~source_odom", Odometry, self.source_odom_callback)
self.init_transform_sub = rospy.Subscriber(
"~initial_base_link_transform", TransformStamped, self.init_transform_callback
) # init_transform is assumed to be transform of init_odom -> base_link
self.pub = rospy.Publisher("~output", Odometry, queue_size=1)
def calculate_init_to_base_link_transform(self, base_odom, base_to_init_transform, stamp):
# tf relationships: init_odom <- base_odom -> base_link
# offset_odom is init_odom -> base_link
# TODO: check timestamps of base_odom and source_odom and fix if necessary
if base_odom == None or base_to_init_transform == None:
return None
base_odom_matrix = make_homogeneous_matrix([getattr(self.base_odom.pose.pose.position, attr) for attr in ["x", "y", "z"]], [getattr(self.base_odom.pose.pose.orientation, attr) for attr in ["x", "y", "z", "w"]]) # base_odom -> body_link
offset_matrix = numpy.linalg.inv(base_to_init_transform).dot(base_odom_matrix)
trans = list(offset_matrix[:3, 3])
rot = list(tf.transformations.quaternion_from_matrix(offset_matrix))
offset_transform = TransformStamped()
offset_transform.header.stamp = stamp
offset_transform.header.frame_id = self.odom_frame
offset_transform.child_frame_id = self.body_frame
offset_transform.transform.translation = Vector3(*trans)
offset_transform.transform.rotation = Quaternion(*rot)
return offset_transform
def calculate_offset(self, source_odom):
# tf relationships: init_odom -> source_odom -> body_link <- init_odom
# offset_odom is init_odom -> source_odom
# TODO: check timestamps of base_odom and source_odom and fix if necessary
if self.initial_base_link_transform is None:
rospy.loginfo("[%s] initial transform is not subscribed yet",
rospy.get_name())
return None
source_odom_matrix = make_homogeneous_matrix([
getattr(source_odom.pose.pose.position, attr)
for attr in ["x", "y", "z"]
], [
getattr(source_odom.pose.pose.orientation, attr)
for attr in ["x", "y", "z", "w"]
]) # source_odom -> body_link
return self.initial_base_link_transform.dot(
numpy.linalg.inv(source_odom_matrix)
) # T(init->src) * T(src->body) = T(init->body)
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 base_to_init_transform_callback(self, msg):
with self.lock:
base_to_init_transform = make_homogeneous_matrix([getattr(msg.transform.translation, attr) for attr in ["x", "y", "z"]], [getattr(msg.transform.rotation, attr) for attr in ["x", "y", "z", "w"]]) # base_odom -> init_odom
self.offset_transform = self.calculate_init_to_base_link_transform(self.base_odom, base_to_init_transform, msg.header.stamp)
if self.offset_transform != None:
self.pub.publish(self.offset_transform)
