def get_fused_pose(self, req):
if req is None:
pose_resp = None
else:
pose = SE2_from_xytheta([req.x, req.y, req.theta])
pose = self.db_kinematics.step(self.encoder_ticks_left_delta_t, self.encoder_ticks_right_delta_t, pose)
trans, theta = translation_angle_from_SE2(pose)
pose_resp = PoseResponse(trans[0], trans[1], theta)
if not self.configure_flag:
self.pose = pose
self.configure_flag = True
if self.configure_flag:
self.pose = self.db_kinematics.step(self.encoder_ticks_left_delta_t, self.encoder_ticks_right_delta_t,
self.pose)
self.encoder_ticks_right_delta_t = 0
self.encoder_ticks_left_delta_t = 0
pose_SE3 = SE3_from_SE2(self.pose)
rot, trans = rotation_translation_from_SE3(pose_SE3)
quaternion_tf = quaternion_from_matrix(pose_SE3)
quaternion = Quaternion(quaternion_tf[0], quaternion_tf[1], quaternion_tf[2], quaternion_tf[3])
translation = Vector3(trans[0], trans[1], trans[2])
trafo = Transform(translation, quaternion)
self.tfs_msg.transform = trafo
if self.vel_left == 0.0 and self.vel_right == 0.0:
self.tfs_msg.header.stamp = rospy.Time.now()
else:
self.tfs_msg.header.stamp = self.encoder_timestamp
self.br.sendTransformMessage(self.tfs_msg)
self.pub_tf_enc_loc.publish(self.tfs_msg)
return pose_resp
def interpolate_measure(measure, alpha):
R = measure.R
t = measure.t
q = g.SE3_from_rotation_translation(R, t)
vel = g.SE3.algebra_from_group(q)
rel = g.SE3.group_from_algebra(vel * alpha)
newR, newt = g.rotation_translation_from_SE3(rel)
return g2o.Isometry3d(newR, newt)
def run(self):
while not rospy.is_shutdown():
if self.image is None:
continue
if not self.camera_info_received:
continue
if self.rectify_flag:
self.rectify_image()
list_pose_tag = self.detect_april_tag()
if not list_pose_tag:
if self.encoder_conf_flag:
trans_map_base_2D, theta_map_base_2D = translation_angle_from_SE2(
self.pose_map_base_SE2)
self.pose_map_base_SE2 = encoder_localization_client(
trans_map_base_2D[0], trans_map_base_2D[1],
theta_map_base_2D)
self.tfs_msg_map_base.transform = self.pose2transform(
SE3_from_SE2(self.pose_map_base_SE2))
self.tfs_msg_map_base.header.stamp = rospy.Time.now()
self.pub_tf_fused_loc.publish(self.tfs_msg_map_base)
self.br_map_base.sendTransformMessage(self.tfs_msg_map_base)
else:
pose_dta_dtc = self.pose_inverse(list_pose_tag[0])
pose_april_cam = self.T_a_dta @ pose_dta_dtc @ self.T_dtc_c
pose_map_base = self.pose_map_at @ pose_april_cam @ self.pose_cam_base
quat_map_base = quaternion_from_matrix(pose_map_base)
euler = euler_from_quaternion(quat_map_base)
theta = euler[2]
rot_map_base, translation_map_base = rotation_translation_from_SE3(
pose_map_base)
pose_map_base_SE2_enc = encoder_localization_client(
translation_map_base[0], translation_map_base[1], theta)
self.encoder_conf_flag = True
self.pose_map_base_SE2 = SE2_from_xytheta(
[translation_map_base[0], translation_map_base[1], theta])
self.tfs_msg_map_base.transform = self.pose2transform(
SE3_from_SE2(self.pose_map_base_SE2))
self.tfs_msg_map_base.header.stamp = rospy.Time.now()
self.pub_tf_fused_loc.publish(self.tfs_msg_map_base)
self.br_map_base.sendTransformMessage(self.tfs_msg_map_base)
self.tfs_msg_map_april.header.stamp = self.image_timestamp
self.static_tf_br_map_april.sendTransform(
self.tfs_msg_map_april)
self.tfs_msg_cam_base.header.stamp = self.image_timestamp
self.static_tf_br_cam_base.sendTransform(self.tfs_msg_cam_base)
self.tfs_msg_april_cam.transform = self.pose2transform(
pose_april_cam)
self.tfs_msg_april_cam.header.stamp = self.image_timestamp
self.br_april_cam.sendTransformMessage(self.tfs_msg_april_cam)
def run(self, event=None):
self.pose = self.db_kinematics.step(self.encoder_ticks_left_delta_t, self.encoder_ticks_right_delta_t, self.pose)
self.encoder_ticks_right_delta_t = 0
self.encoder_ticks_left_delta_t = 0
pose_SE3 = SE3_from_SE2(self.pose)
rot, trans = rotation_translation_from_SE3(pose_SE3)
quaternion_tf = quaternion_from_matrix(pose_SE3)
quaternion = Quaternion(quaternion_tf[0], quaternion_tf[1], quaternion_tf[2], quaternion_tf[3])
translation = Vector3(trans[0], trans[1], trans[2])
trafo = Transform(translation, quaternion)
self.tfs_msg.transform = trafo
if self.vel_left == 0.0 and self.vel_right == 0.0:
self.tfs_msg.header.stamp = rospy.Time.now()
else:
self.tfs_msg.header.stamp = self.encoder_timestamp
self.br.sendTransformMessage(self.tfs_msg)
self.pub_tf_enc_loc.publish(self.tfs_msg)
def __init__(self, node_name):
# Initialize the DTROS parent class
super(EncoderLocalizationNode, self).__init__(node_name=node_name,node_type=NodeType.GENERIC)
self.veh_name = rospy.get_namespace().strip("/")
self.radius = rospy.get_param(f'/{self.veh_name}/kinematics_node/radius', 100)
self.baseline = 0.0968
x_init = rospy.get_param(f'/{self.veh_name}/{node_name}/x_init', 100)
self.pose = SE2_from_xytheta([x_init, 0, np.pi])
self.db_kinematics = DuckiebotKinematics(radius=self.radius, baseline=self.baseline)
self.vel_left = 0.0
self.vel_right = 0.0
self.encoder_ticks_left_total = 0
self.encoder_ticks_left_delta_t = 0
self.encoder_ticks_right_total = 0
self.encoder_ticks_right_delta_t = 0
self.encoder_timestamp = rospy.Time.now()
self.max_number_ticks = 135
init_pose_SE3 = SE3_from_SE2(self.pose)
rot, trans = rotation_translation_from_SE3(init_pose_SE3)
quaternion_tf = quaternion_from_matrix(init_pose_SE3)
quaternion = Quaternion(quaternion_tf[0], quaternion_tf[1], quaternion_tf[2], quaternion_tf[3])
translation = Vector3(trans[0], trans[1], trans[2])
trafo = Transform(translation, quaternion)
self.tfs_msg = TransformStamped()
self.tfs_msg.header.frame_id = 'map'
self.tfs_msg.header.stamp = self.encoder_timestamp
self.tfs_msg.child_frame_id = 'encoder_baselink'
self.tfs_msg.transform = trafo
self.br = tf.TransformBroadcaster()
self.sub_executed_commands = rospy.Subscriber(f'/{self.veh_name}/wheels_driver_node/wheels_cmd_executed',
WheelsCmdStamped, self.cb_executed_commands)
self.sub_encoder_ticks_left = rospy.Subscriber(f'/{self.veh_name}/left_wheel_encoder_node/tick',
WheelEncoderStamped, self.callback_left)
self.sub_encoder_ticks_right = rospy.Subscriber(f'/{self.veh_name}/right_wheel_encoder_node/tick',
WheelEncoderStamped, self.callback_right)
self.pub_tf_enc_loc = rospy.Publisher(f'/{self.veh_name}/{node_name}/transform_stamped',
TransformStamped, queue_size=10)
def interpolate(self, old_time_stamp, new_time_stamp, measure):
"""Given a timestamp new_time_stamp at which an odometry message was
received and the timestamp old_time_stamp of the last odometry
message previously received, it might be the case that other
messages, of non-odometry type, have been received in the time
interval between these two timestamps. For instance, a Duckiebot
might be seen by a watchtower at two timestamps time_stamp1 and
time_stamp2 s.t. old_time_stamp < time_stamp1 < time_stamp2 <
new_time_stamp. This function creates edges in the graph between each
pair of vertices at consecutive timestamps (e.g.
old_time_stamp-->time_stamp1, time_stamp1-->time_stamp2,
time_stamp2-->new_time_stamp). The relative transform between each
pair of vertices is assigned by performing a linear interpolation in
the Lie algebra, based on the transform between old_time_stamp and
new_time_stamp (contained in the odometry message) and on the
relative time difference (e.g. time_stamp2 - time_stamp1).
Args:
old_time_stamp: Timestamp of the last odometry message that was
received before the current odometry message, for
the node of type node_type and ID node_id.
new_time_stamp: Timestamp of the current odometry message.
node_type: Type of the node.
node_id: ID of the node.
measure: Transform contained in the current odometry message,
between timestamp old_time_stamp and time_stamp
new_time_stamp.
"""
# Timestamps for which the interpolation should be performed. Note: also
# old_time_stamp and new_time_stamp are included.
with self.node_lock:
to_interpolate = {
time_stamp
for time_stamp in self.time_stamps_to_indices if
(time_stamp >= old_time_stamp and time_stamp <= new_time_stamp)
}
# Sort the time stamps.
sorted_time_stamps = sorted(to_interpolate)
# print("perfoming interpolation on %d nodes" % len(sorted_time_stamps))
# Find the total time (time between the last and the first timestamp).
total_delta_t = float(sorted_time_stamps[-1] - sorted_time_stamps[0])
if (total_delta_t == 0.0):
print("in interpolate, delta t is 0.0, with %s and list is:" %
self.node_id)
print(to_interpolate)
print("new_time_stamp is %f and old time stamp is %f" %
(old_time_stamp, new_time_stamp))
print(self.time_stamps_to_indices)
# Perform a linear interpolation in the Lie algebra associated to SE3
# group defined by the transform.
R = measure.R
t = measure.t
q = g.SE3_from_rotation_translation(R, t)
vel = g.SE3.algebra_from_group(q)
cumulative_alpha = 0.0
interpolation_list = []
for i in range(0, len(sorted_time_stamps) - 1):
# Find the time interval between each timestamp and the subsequent
# one and linearly interpolate accordingly in the algebra.
partial_delta_t = float(sorted_time_stamps[i + 1] -
sorted_time_stamps[i])
alpha = partial_delta_t / total_delta_t
cumulative_alpha += alpha
rel = g.SE3.group_from_algebra(vel * alpha)
newR, newt = g.rotation_translation_from_SE3(rel)
interpolated_measure = g2o.Isometry3d(newR, newt)
vertex0_index = self.get_g2o_index(sorted_time_stamps[i])
vertex1_index = self.get_g2o_index(sorted_time_stamps[i + 1])
# Add an edge to the graph, connecting each timestamp to the one
# following it and using the relative transform obtained by
# interpolation.
interpolation_list.append(
(vertex0_index, vertex1_index, interpolated_measure, 0.1))
if (cumulative_alpha != 1.0):
pass
for args in interpolation_list:
self.duckietown_graph.graph.add_edge(*args)
def pose_inverse(self, pose: SE3) -> Transform:
rot, trans = rotation_translation_from_SE3(pose)
rot_inv = rot.T
trans_inv = -rot_inv @ trans
return SE3_from_rotation_translation(rot_inv, trans_inv)
def pose2transform(self, pose: SE3) -> Transform:
rot, trans = rotation_translation_from_SE3(pose)
quat_tf = quaternion_from_matrix(pose)
quaternion = Quaternion(quat_tf[0], quat_tf[1], quat_tf[2], quat_tf[3])
translation = Vector3(trans[0], trans[1], trans[2])
return Transform(translation, quaternion)