def source_odom_callback(self, msg):
with self.lock:
# calculate camera transform
current_camera_to_base = self.calculate_camera_to_base_transform(msg.header.stamp)
if current_camera_to_base == None:
return
if self.initial_matrix == None:
self.initial_matrix = numpy.linalg.inv(current_camera_to_base)
camera_relative_base_transformation = numpy.dot(self.initial_matrix, current_camera_to_base) # base_link transformation in camera coords
# calculate offseted odometry
source_odom_matrix = make_homogeneous_matrix([msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z],
[msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w])
new_odom_matrix = camera_relative_base_transformation.dot(source_odom_matrix)
# make odometry msg. twist is copied from source_odom
new_odom = copy.deepcopy(msg)
new_odom.header.frame_id = self.odom_frame
new_odom.child_frame_id = self.base_link_frame
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)))
new_pose_cov_matrix = numpy.matrix(new_odom.pose.covariance).reshape(6, 6)
rotation_matrix = camera_relative_base_transformation[: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)
def base_odom_callback(self, msg):
with self.lock:
try:
self.listener.waitForTransform(self.map_frame, msg.header.frame_id, msg.header.stamp, rospy.Duration(3))
(trans,rot) = self.listener.lookupTransform(self.map_frame, msg.header.frame_id, msg.header.stamp) # current map->base_odom transform
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
return
base_odom_homogeneous_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"]]) # base_odom -> body
map_to_base_odom_homogeneous_matrix = make_homogeneous_matrix(trans, rot) # map -> base_odom
slam_odom_matrix = map_to_base_odom_homogeneous_matrix.dot(base_odom_homogeneous_matrix) # map -> body
self.slam_odom = Odometry()
self.slam_odom.pose.pose.position = Point(*list(slam_odom_matrix[:3, 3]))
self.slam_odom.pose.pose.orientation = Quaternion(*list(tf.transformations.quaternion_from_matrix(slam_odom_matrix)))
self.slam_odom.header.stamp = msg.header.stamp
self.slam_odom.header.frame_id = self.map_frame
self.slam_odom.child_frame_id = msg.child_frame_id
# calculate covariance
# use covariance of base_odom.pose and only transform it to new coords from tf
# TODO: calc covariance in correct way, but what is that ...?
rotation_matrix = map_to_base_odom_homogeneous_matrix[:3, :3]
original_pose_cov_matrix = numpy.matrix(msg.pose.covariance).reshape(6, 6)
pose_cov_matrix = numpy.zeros((6, 6))
pose_cov_matrix[:3, :3] = (rotation_matrix.T).dot(original_pose_cov_matrix[:3, :3].dot(rotation_matrix))
pose_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot(original_pose_cov_matrix[3:6, 3:6].dot(rotation_matrix))
self.slam_odom.pose.covariance = numpy.array(pose_cov_matrix).reshape(-1,).tolist()
# twist is local and transform same as covariance
new_velocity = numpy.dot(rotation_matrix, numpy.array([[msg.twist.twist.linear.x],
[msg.twist.twist.linear.y],
[msg.twist.twist.linear.z]]))
new_omega = numpy.dot(rotation_matrix, numpy.array([[msg.twist.twist.angular.x],
[msg.twist.twist.angular.y],
[msg.twist.twist.angular.z]]))
original_twist_cov_matrix = numpy.matrix(msg.twist.covariance).reshape(6, 6)
twist_cov_matrix = numpy.zeros((6, 6))
twist_cov_matrix[:3, :3] = (rotation_matrix.T).dot(original_twist_cov_matrix[:3, :3].dot(rotation_matrix))
twist_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot(original_twist_cov_matrix[3:6, 3:6].dot(rotation_matrix))
self.slam_odom.twist = TwistWithCovariance(Twist(Vector3(*new_velocity[:, 0]), Vector3(*new_omega[:, 0])), twist_cov_matrix.reshape(-1,).tolist())
if not self.pub_only_map_updated:
self.pub.publish(self.slam_odom)
def base_odom_callback(self, msg):
with self.lock:
try:
self.listener.waitForTransform(self.map_frame, msg.header.frame_id, msg.header.stamp, rospy.Duration(3))
(trans,rot) = self.listener.lookupTransform(self.map_frame, msg.header.frame_id, msg.header.stamp) # current map->base_odom transform
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
return
base_odom_homogeneous_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"]]) # base_odom -> body
map_to_base_odom_homogeneous_matrix = make_homogeneous_matrix(trans, rot) # map -> base_odom
slam_odom_matrix = map_to_base_odom_homogeneous_matrix.dot(base_odom_homogeneous_matrix) # map -> body
self.slam_odom = Odometry()
self.slam_odom.pose.pose.position = Point(*list(slam_odom_matrix[:3, 3]))
self.slam_odom.pose.pose.orientation = Quaternion(*list(tf.transformations.quaternion_from_matrix(slam_odom_matrix)))
self.slam_odom.header.stamp = msg.header.stamp
self.slam_odom.header.frame_id = self.map_frame
self.slam_odom.child_frame_id = msg.child_frame_id
# calculate covariance
# use covariance of base_odom.pose and only transform it to new coords from tf
# TODO: calc covariance in correct way, but what is that ...?
rotation_matrix = map_to_base_odom_homogeneous_matrix[:3, :3]
original_pose_cov_matrix = numpy.matrix(msg.pose.covariance).reshape(6, 6)
pose_cov_matrix = numpy.zeros((6, 6))
pose_cov_matrix[:3, :3] = (rotation_matrix.T).dot(original_pose_cov_matrix[:3, :3].dot(rotation_matrix))
pose_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot(original_pose_cov_matrix[3:6, 3:6].dot(rotation_matrix))
self.slam_odom.pose.covariance = numpy.array(pose_cov_matrix).reshape(-1,).tolist()
# twist is local and transform same as covariance
new_velocity = numpy.dot(rotation_matrix, numpy.array([[msg.twist.twist.linear.x],
[msg.twist.twist.linear.y],
[msg.twist.twist.linear.z]]))
new_omega = numpy.dot(rotation_matrix, numpy.array([[msg.twist.twist.angular.x],
[msg.twist.twist.angular.y],
[msg.twist.twist.angular.z]]))
original_twist_cov_matrix = numpy.matrix(msg.twist.covariance).reshape(6, 6)
twist_cov_matrix = numpy.zeros((6, 6))
twist_cov_matrix[:3, :3] = (rotation_matrix.T).dot(original_twist_cov_matrix[:3, :3].dot(rotation_matrix))
twist_cov_matrix[3:6, 3:6] = (rotation_matrix.T).dot(original_twist_cov_matrix[3:6, 3:6].dot(rotation_matrix))
self.slam_odom.twist = TwistWithCovariance(Twist(Vector3(*new_velocity[:, 0]), Vector3(*new_omega[:, 0])), twist_cov_matrix.reshape(-1,).tolist())
if not self.pub_only_map_updated:
self.pub.publish(self.slam_odom)
def calculate_camera_to_base_transform(self, stamp):
try:
(trans,rot) = self.listener.lookupTransform(self.camera_frame, self.base_link_frame, stamp)
except:
try:
rospy.logwarn("[%s] failed to solve camera_to_base tf in %f. Use rospy.Time(0): %s to %s", rospy.get_name(), stamp.to_sec(), self.camera_frame, self.base_link_frame)
(trans,rot) = self.listener.lookupTransform(self.camera_frame, self.base_link_frame, rospy.Time(0))
except:
rospy.logwarn("[%s] failed to solve camera_to_base tf: %s to %s", rospy.get_name(), self.camera_frame, self.base_link_frame)
return None
camera_to_base_link = make_homogeneous_matrix(trans, rot) # camera -> base_link
return camera_to_base_link
def source_odom_callback(self, msg):
with self.lock:
# calculate camera transform
current_camera_to_base = self.calculate_camera_to_base_transform(
msg.header.stamp)
if current_camera_to_base is None:
return
if self.initial_matrix is None:
self.initial_matrix = numpy.linalg.inv(current_camera_to_base)
camera_relative_base_transformation = numpy.dot(
self.initial_matrix, current_camera_to_base
) # base_link transformation in camera coords
# calculate offseted odometry
source_odom_matrix = make_homogeneous_matrix([
msg.pose.pose.position.x, msg.pose.pose.position.y,
msg.pose.pose.position.z
], [
msg.pose.pose.orientation.x, msg.pose.pose.orientation.y,
msg.pose.pose.orientation.z, msg.pose.pose.orientation.w
])
new_odom_matrix = camera_relative_base_transformation.dot(
source_odom_matrix)
# make odometry msg. twist is copied from source_odom
new_odom = copy.deepcopy(msg)
new_odom.header.frame_id = self.odom_frame
new_odom.child_frame_id = self.base_link_frame
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)))
new_pose_cov_matrix = numpy.matrix(
new_odom.pose.covariance).reshape(6, 6)
rotation_matrix = camera_relative_base_transformation[: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)
def calculate_camera_to_base_transform(self, stamp):
try:
(trans,
rot) = self.listener.lookupTransform(self.camera_frame,
self.base_link_frame, stamp)
except:
try:
rospy.logwarn(
"[%s] failed to solve camera_to_base tf in %f. Use rospy.Time(0): %s to %s",
rospy.get_name(), stamp.to_sec(), self.camera_frame,
self.base_link_frame)
(trans,
rot) = self.listener.lookupTransform(self.camera_frame,
self.base_link_frame,
rospy.Time(0))
except:
rospy.logwarn(
"[%s] failed to solve camera_to_base tf: %s to %s",
rospy.get_name(), self.camera_frame, self.base_link_frame)
return None
camera_to_base_link = make_homogeneous_matrix(
trans, rot) # camera -> base_link
return camera_to_base_link
