def handle_watchtower_message(self, node_id0, node_id1, data, time_stamp, pose_error=None):
"""Processes a message containing the pose of an object seen by a
watchtower and adds an edge to the graph. If the object seen is a
Duckiebot, adjusts the pose accordingly.
Args:
node_id0: ID of the object (watchtower) that sees the April tag of the
other object.
node_id1: ID of the object whose April tag is seen by the watchtower.
transform: Transform contained in the ROS message.
time_stamp: Timestamp associated to the ROS message.
"""
transform = get_transform_from_data(data)
# Get type of the object seen.
type_of_object_seen = get_node_type(node_id1)
space_dev = self.default_variance["watchtower"]["position_deviation"]
angle_dev = self.default_variance["watchtower"]["angle_deviation"]
if(pose_error != None):
measure_information = create_info_matrix(
space_dev * pose_error, angle_dev * pose_error)
else:
measure_information = create_info_matrix(space_dev, angle_dev)
if (type_of_object_seen == "duckiebot"):
# print("watzchtower %s is seing duckiebot %s" %
# (node_id0, node_id1))
# In case of Duckiebot the pose needs to be adjusted to take into
# account the pose of the April tag w.r.t. the base frame of the
# Duckiebot.
t = [0.0, 0.0, 0.055]
z_angle = 90
x_angle = 178
z_angle = np.deg2rad(z_angle)
x_angle = np.deg2rad(x_angle)
R_z = g.rotation_from_axis_angle(np.array([0, 0, 1]), z_angle)
R_x = g.rotation_from_axis_angle(np.array([1, 0, 0]), x_angle)
R = np.matmul(R_x, R_z) # verified!
H_apriltag_to_base = g2o.Isometry3d(R, t)
transform = transform * H_apriltag_to_base.inverse()
# Add edge to the graph.
return self.resampler.handle_watchtower_edge(node_id0, node_id1, transform, time_stamp, measure_information=measure_information, is_duckiebot=True)
else:
# Add edge to the graph.
if node_id0 not in self.edge_counters:
self.edge_counters[node_id0] = dict()
if node_id1 not in self.edge_counters[node_id0]:
self.edge_counters[node_id0][node_id1] = 0
if(self.edge_counters[node_id0][node_id1] < self.max_number_same_edge):
self.resampler.handle_watchtower_edge(
node_id0, node_id1, transform, time_stamp)
self.edge_counters[node_id0][node_id1] += 1
else:
a = random.randint(0, self.rejection_sampling_int)
if(a == 0):
self.edge_counters[node_id0][node_id1] += 1
return self.resampler.handle_watchtower_edge(node_id0, node_id1, transform, time_stamp, measure_information=measure_information)
def handle_duckiebot_message(self, node_id0, node_id1, transform, time_stamp):
"""Processes a message containing the pose of an object seen by a
Duckiebot and adds an edge to the graph. Note: we assume that a
Duckiebot cannot see the April tag of another Duckiebot, so no
adjustment based on the object seen is needed.
Args:
node_id0: ID of the object (Duckiebot) that sees the April tag of the
other object.
node_id1: ID of the object whose April tag is seen by the Duckiebot.
transform: Transform contained in the ROS message.
time_stamp: Timestamp associated to the ROS message.
"""
# Get type of the object that sees the other object, for a sanity check.
type_of_object_seeing = node_id0.split("_")[0]
if (type_of_object_seeing == "duckiebot"):
# The pose needs to be adjusted to take into account the relative
# pose of the camera on the Duckiebot w.r.t. to the base frame of
# the Duckiebot.
t = [0.075, 0.0, 0.025]
# This angle is an estimate of the angle by which the plastic
# support that holds the camera is tilted.
x_angle = -102
z_angle = -90
x_angle = np.deg2rad(x_angle)
z_angle = np.deg2rad(z_angle)
R_x = g.rotation_from_axis_angle(np.array([1, 0, 0]), x_angle)
R_z = g.rotation_from_axis_angle(np.array([0, 0, 1]), z_angle)
R = np.matmul(R_z, R_x) # verified
H_base_to_camera = g2o.Isometry3d(R, t)
transform = H_base_to_camera * transform
else:
print("This should not be here! %s " % node_id0)
def add_vertex(self,
node_id,
theta,
position,
is_initial_floor_tag=False,
fixed=False,
time_stamp=0.0):
"""Adds a node with an ID given in the format outputted from the
transform_listener and with a given pose to the g2o graph.
TODO : add a more general add_vertex function that takes a 3D pose and not only a 2D one.
Args:
node_id: ID of the node whose pose should be added as a node
in the graph, in the format <node_type>_<node_id>.
theta: Angle of the 2D rotation around the z axis defines the
pose of the node.
position: 2D translation vector (x, y) that define the pose of the node.
is_initial_floor_tag: True if the node is an initial floor tag,
False otherwise.
fixed: True if the node should be added as a fixed node (i.e.,
not optimizable) in the graph, False otherwise.
time_stamp: Timestamp.
"""
# Adds (assigns) a timestamp to a node, by giving it a 'local index'
# w.r.t. the node.
node = self.get_node(node_id)
added = node.add_timestamp(time_stamp)
if (not added):
print(
"add_vertex did not add : node %s at time %f as already there"
% (node_id, time_stamp))
else:
# Translation vector, z coordinate does not vary.
position = [position[0], position[1], 0.0]
# Rotation around the z axis of and angle theta.
R = g.rotation_from_axis_angle(np.array([0, 0, 1]),
np.deg2rad(theta))
if (is_initial_floor_tag):
# For initial floor tags, apply a further rotation of 180
# degrees around the x axis, so that the orientation of the z
# axis is reverted.
R2 = g.rotation_from_axis_angle(np.array([1, 0, 0]),
np.deg2rad(180))
R = np.matmul(R, R2)
# Add vertex with pose and ID in the right format to the g2o graph.
vertex_pose = g2o.Isometry3d(R, position)
vertex_index = node.get_g2o_index(time_stamp)
self.graph.add_vertex(vertex_index, vertex_pose, fixed=fixed)
if (is_initial_floor_tag):
# For initial floor tags, add an edge between the reference
# vertex and the newly-added vertex for the pose of the tag.
self.graph.add_edge(vertex0_id=0,
vertex1_id=vertex_index,
measure=vertex_pose)
def handle_watchtower_message(self, node_id0, node_id1, transform,
time_stamp):
"""Processes a message containing the pose of an object seen by a
watchtower and adds an edge to the graph. If the object seen is a
Duckiebot, adjusts the pose accordingly.
Args:
node_id0: ID of the object (watchtower) that sees the April tag of the
other object.
node_id1: ID of the object whose April tag is seen by the watchtower.
transform: Transform contained in the ROS message.
time_stamp: Timestamp associated to the ROS message.
"""
# Get type of the object seen.
type_of_object_seen = node_id1.split("_")[0]
if (type_of_object_seen == "duckiebot"):
print("watzchtower %s is seing duckiebot %s" %
(node_id0, node_id1))
# In case of Duckiebot the pose needs to be adjusted to take into
# account the pose of the April tag w.r.t. the base frame of the
# Duckiebot.
t = [0.0, 0.0, 0.055]
z_angle = 90
x_angle = 178
z_angle = np.deg2rad(z_angle)
x_angle = np.deg2rad(x_angle)
R_z = g.rotation_from_axis_angle(np.array([0, 0, 1]), z_angle)
R_x = g.rotation_from_axis_angle(np.array([1, 0, 0]), x_angle)
R = np.matmul(R_x, R_z) # verified!
H_apriltag_to_base = g2o.Isometry3d(R, t)
transform = transform * H_apriltag_to_base.inverse()
# Add edge to the graph.
return self.pose_graph.add_edge(node_id0, node_id1, transform,
time_stamp)
else:
# Add edge to the graph.
if node_id0 not in self.edge_counters:
self.edge_counters[node_id0] = dict()
if node_id1 not in self.edge_counters[node_id0]:
self.edge_counters[node_id0][node_id1] = 0
if (self.edge_counters[node_id0][node_id1] <
self.max_number_same_edge):
self.pose_graph.add_edge(node_id0, node_id1, transform,
time_stamp)
self.edge_counters[node_id0][node_id1] += 1
else:
a = random.randint(0, self.rejection_sampling_int)
if (a == 0):
self.edge_counters[node_id0][node_id1] += 1
return self.pose_graph.add_edge(node_id0, node_id1,
transform, time_stamp)
def create_measure(self):
t = self.position
roll_angle = np.deg2rad(self.rotation[0])
pitch_angle = np.deg2rad(self.rotation[1])
yaw_angle = np.deg2rad(self.rotation[2])
R_roll = g.rotation_from_axis_angle(np.array([0, 1, 0]), roll_angle)
R_pitch = g.rotation_from_axis_angle(np.array([1, 0, 0]), pitch_angle)
R_yaw = g.rotation_from_axis_angle(np.array([0, 0, 1]), yaw_angle)
R = np.matmul(R_roll, np.matmul(R_pitch, R_yaw))
self.measure = g2o.Isometry3d(R, t)
def handle_duckiebot_message(self,
node_id0,
node_id1,
data,
time_stamp,
pose_error=None):
"""Processes a message containing the pose of an object seen by a
Duckiebot and adds an edge to the graph. Note: we assume that a
Duckiebot cannot see the April tag of another Duckiebot, so no
adjustment based on the object seen is needed.
Args:
node_id0: ID of the object (Duckiebot) that sees the April tag of the
other object.
node_id1: ID of the object whose April tag is seen by the Duckiebot.
transform: Transform contained in the ROS message.
time_stamp: Timestamp associated to the ROS message.
"""
transform = get_transform_from_data(data)
space_dev = self.default_variance["duckiebot"]["position_deviation"]
angle_dev = self.default_variance["duckiebot"]["angle_deviation"]
if (pose_error != None):
measure_information = create_info_matrix(space_dev * pose_error,
angle_dev * pose_error)
else:
measure_information = create_info_matrix(space_dev, angle_dev)
# Get type of the object that sees the other object, for a sanity check.
type_of_object_seeing = node_id0.split("_")[0]
if (type_of_object_seeing == "duckiebot"):
# The pose needs to be adjusted to take into account the relative
# pose of the camera on the Duckiebot w.r.t. to the base frame of
# the Duckiebot.
t = [0.075, 0.0, 0.025]
# This angle is an estimate of the angle by which the plastic
# support that holds the camera is tilted.
x_angle = -102
z_angle = -90
x_angle = np.deg2rad(x_angle)
z_angle = np.deg2rad(z_angle)
R_x = g.rotation_from_axis_angle(np.array([1, 0, 0]), x_angle)
R_z = g.rotation_from_axis_angle(np.array([0, 0, 1]), z_angle)
R = np.matmul(R_z, R_x) # verified
H_base_to_camera = g2o.Isometry3d(R, t)
transform = H_base_to_camera * transform
# Add edge to the graph.
# return self.resampler.handle_duckiebot_edge(node_id0, node_id1, transform, time_stamp, measure_information)
else:
print("This should not be here! %s " % node_id0)
def assert_orthogonal_test():
for s in directions_sequence():
axis = any_orthogonal_direction(s)
angle = np.pi / 2
R = rotation_from_axis_angle(axis, angle)
s2 = np.dot(R, s)
assert_orthogonal(s, s2)
def assert_orthogonal_test():
for s in directions_sequence():
axis = any_orthogonal_direction(s)
angle = np.pi / 2
R = rotation_from_axis_angle(axis, angle)
s2 = np.dot(R, s)
assert_orthogonal(s, s2)
def test_distances_rotations(self):
for axis, angle in axis_angle_sequence():
s = random_direction()
R = rotation_from_axis_angle(axis, angle)
s2 = np.dot(R, s)
dist = geodesic_distance_on_sphere(s, s2)
# Note: this is == only if axis is orthogonal to s
assert dist <= angle
def test_distances_rotations(self):
for axis, angle in axis_angle_sequence():
s = random_direction()
R = rotation_from_axis_angle(axis, angle)
s2 = np.dot(R, s)
dist = geodesic_distance_on_sphere(s, s2)
# Note: this is == only if axis is orthogonal to s
assert dist <= angle
def test_rotation_from_axis_angle2(self):
for axis, angle in axis_angle_sequence():
R1 = rotation_from_axis_angle(axis, angle)
R2 = rotation_from_axis_angle2(axis, angle)
if False:
s1, a1 = axis_angle_from_rotation(R1)
s2, a2 = axis_angle_from_rotation(R2)
print('Origi: %s around %s' % (angle, axis))
print('First: %s around %s' % (a1, s1))
print('Secnd: %s around %s' % (a2, s2))
assert_allclose(R1, R2)
def test_rotation_from_axis_angle2(self):
for axis, angle in axis_angle_sequence():
R1 = rotation_from_axis_angle(axis, angle)
R2 = rotation_from_axis_angle2(axis, angle)
if False:
s1, a1 = axis_angle_from_rotation(R1)
s2, a2 = axis_angle_from_rotation(R2)
print('Origi: %s around %s' % (angle, axis))
print('First: %s around %s' % (a1, s1))
print('Secnd: %s around %s' % (a2, s2))
assert_allclose(R1, R2)
def __init__(self, node_name):
super(FusedLocalizationNode, 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.rectify_flag = rospy.get_param(
f'/{self.veh_name}/{node_name}/rectify', 100)
self.encoder_conf_flag = False
self.bridge = CvBridge()
self.image = None
self.image_timestamp = rospy.Time.now()
self.cam_info = None
self.camera_info_received = False
self.newCameraMatrix = None
self.at_detector = Detector(families='tag36h11',
nthreads=1,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.25,
debug=0)
trans_base_cam = np.array([0.0582, 0.0, 0.1072])
rot_base_cam = rotation_from_axis_angle(np.array([0, 1, 0]),
np.radians(15))
rot_cam_base = rot_base_cam.T
trans_cam_base = -rot_cam_base @ trans_base_cam
self.pose_cam_base = SE3_from_rotation_translation(
rot_cam_base, trans_cam_base)
self.tfs_msg_cam_base = TransformStamped()
self.tfs_msg_cam_base.header.frame_id = 'camera'
self.tfs_msg_cam_base.header.stamp = rospy.Time.now()
self.tfs_msg_cam_base.child_frame_id = 'at_baselink'
self.tfs_msg_cam_base.transform = self.pose2transform(
self.pose_cam_base)
self.static_tf_br_cam_base = tf2_ros.StaticTransformBroadcaster()
translation_map_at = np.array([0.0, 0.0, 0.08])
rot_map_at = np.eye(3)
self.pose_map_at = SE3_from_rotation_translation(
rot_map_at, translation_map_at)
self.tfs_msg_map_april = TransformStamped()
self.tfs_msg_map_april.header.frame_id = 'map'
self.tfs_msg_map_april.header.stamp = rospy.Time.now()
self.tfs_msg_map_april.child_frame_id = 'apriltag'
self.tfs_msg_map_april.transform = self.pose2transform(
self.pose_map_at)
self.static_tf_br_map_april = tf2_ros.StaticTransformBroadcaster()
self.tfs_msg_april_cam = TransformStamped()
self.tfs_msg_april_cam.header.frame_id = 'apriltag'
self.tfs_msg_april_cam.header.stamp = rospy.Time.now()
self.tfs_msg_april_cam.child_frame_id = 'camera'
self.br_april_cam = tf.TransformBroadcaster()
self.tfs_msg_map_base = TransformStamped()
self.tfs_msg_map_base.header.frame_id = 'map'
self.tfs_msg_map_base.header.stamp = rospy.Time.now()
self.tfs_msg_map_base.child_frame_id = 'fused_baselink'
self.br_map_base = tf.TransformBroadcaster()
self.pose_map_base_SE2 = SE2_from_xytheta([x_init, 0, np.pi])
R_x_c = rotation_from_axis_angle(np.array([1, 0, 0]), np.pi / 2)
R_z_c = rotation_from_axis_angle(np.array([0, 0, 1]), np.pi / 2)
R_y_a = rotation_from_axis_angle(np.array([0, 1, 0]), -np.pi / 2)
R_z_a = rotation_from_axis_angle(np.array([0, 0, 1]), np.pi / 2)
R_dtc_c = R_x_c @ R_z_c
R_a_dta = R_y_a @ R_z_a
self.T_dtc_c = SE3_from_rotation_translation(R_dtc_c,
np.array([0, 0, 0]))
self.T_a_dta = SE3_from_rotation_translation(R_a_dta,
np.array([0, 0, 0]))
self.sub_image_comp = rospy.Subscriber(
f'/{self.veh_name}/camera_node/image/compressed',
CompressedImage,
self.image_cb,
buff_size=10000000,
queue_size=1)
self.sub_cam_info = rospy.Subscriber(
f'/{self.veh_name}/camera_node/camera_info',
CameraInfo,
self.cb_camera_info,
queue_size=1)
self.pub_tf_fused_loc = rospy.Publisher(
f'/{self.veh_name}/{node_name}/transform_stamped',
TransformStamped,
queue_size=10)
