def test_validate_snapshot_disjoint():
"""Tests that a frame tree that is disconnected does not validate."""
snapshot_text = """child_to_parent_edge_map {
key: "beta"
value: {
parent_frame_name: "alpha"
}
}
child_to_parent_edge_map {
key: "delta"
value: {
parent_frame_name: "gamma"
}
}
child_to_parent_edge_map {
key: "alpha"
value: {
parent_frame_name: ""
}
}
child_to_parent_edge_map {
key: "gamma"
value: {
parent_frame_name: ""
}
}"""
with pytest.raises(frame_helpers.ValidateFrameTreeDisjointError):
frame_helpers.validate_frame_tree_snapshot(
_create_snapshot(snapshot_text))
def test_validate_snapshot_empty_key_name():
"""Tests that a frame tree with an empty child frame name does not validate."""
snapshot_text = """child_to_parent_edge_map {
key: ""
value: {
parent_frame_name: "alpha"
}
}"""
with pytest.raises(frame_helpers.ValidateFrameTreeError):
frame_helpers.validate_frame_tree_snapshot(_create_snapshot(snapshot_text))
def test_validate_snapshot_unknown_parent():
"""Tests that a frame tree with an unknown parent does not validate."""
snapshot_text = """child_to_parent_edge_map {
key: "beta"
value: {
parent_frame_name: "foo"
}
}"""
with pytest.raises(frame_helpers.ValidateFrameTreeUnknownFrameError):
frame_helpers.validate_frame_tree_snapshot(_create_snapshot(snapshot_text))
def test_validate_snapshot_single_edge_cycle():
"""Tests that a frame tree with a single edge that has a cycle does not validate."""
snapshot_text = """child_to_parent_edge_map {
key: "alpha"
value: {
parent_frame_name: "alpha"
}
}"""
with pytest.raises(frame_helpers.ValidateFrameTreeCycleError):
frame_helpers.validate_frame_tree_snapshot(_create_snapshot(snapshot_text))
def test_frame_tree_math_single_edge():
snapshot_text = """child_to_parent_edge_map {
key: "beta"
value: {
parent_frame_name: "alpha"
parent_tform_child: {
position: {
x: 10
y: 0
z: 0
}
}
}
}
child_to_parent_edge_map {
key: "alpha"
value: {
parent_frame_name: ""
}
}"""
frame_tree = _create_snapshot(snapshot_text)
assert frame_helpers.validate_frame_tree_snapshot(frame_tree)
assert _do_poses_match(
10, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'alpha', 'beta'))
assert _do_poses_match(
-10, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'beta', 'alpha'))
assert _do_poses_match(
0, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'alpha', 'alpha'))
assert _do_poses_match(
0, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'beta', 'beta'))
assert not frame_helpers.get_a_tform_b(frame_tree, 'omega', 'alpha')
assert not frame_helpers.get_a_tform_b(frame_tree, 'alpha', 'omega')
assert not frame_helpers.get_a_tform_b(frame_tree, 'omega', 'omega')
assert not frame_helpers.get_a_tform_b(frame_tree, 'omaga', 'psi')
def test_validate_snapshot_single_child():
"""Tests that a single edge tree is valid."""
snapshot_text = """child_to_parent_edge_map {
key: "beta"
value: {
parent_frame_name: "alpha"
}
}
child_to_parent_edge_map {
key: "alpha"
value: {
parent_frame_name: ""
}
}"""
assert frame_helpers.validate_frame_tree_snapshot(_create_snapshot(snapshot_text))
def test_validate_snapshot_linear_chain():
"""Tests that a tree with a linear chain is parsed correctly."""
snapshot_text = """child_to_parent_edge_map {
key: "beta"
value: {
parent_frame_name: "alpha"
}
}
child_to_parent_edge_map {
key: "gamma"
value: {
parent_frame_name: "beta"
}
}
child_to_parent_edge_map {
key: "alpha"
value: {
parent_frame_name: ""
}
}"""
assert frame_helpers.validate_frame_tree_snapshot(_create_snapshot(snapshot_text))
def test_validate_snapshot_two_children():
"""Tests that a tree with two children off of the root is valid."""
snapshot_text = """child_to_parent_edge_map {
key: "beta"
value: {
parent_frame_name: "alpha"
}
}
child_to_parent_edge_map {
key: "gamma"
value: {
parent_frame_name: "alpha"
}
}
child_to_parent_edge_map {
key: "alpha"
value: {
parent_frame_name: ""
}
}"""
assert frame_helpers.validate_frame_tree_snapshot(_create_snapshot(snapshot_text))
def test_validate_snapshot_empty_frame_tree():
"""Tests that an empty frame tree does not validate."""
snapshot_text = """"""
with pytest.raises(frame_helpers.ValidateFrameTreeError):
frame_helpers.validate_frame_tree_snapshot(
_create_snapshot(snapshot_text))
def test_express_velocity_types():
snapshot_text = """
child_to_parent_edge_map {
key: "vision"
value: {
parent_frame_name: "body"
parent_tform_child: {
position: {
x: 1
z: 10
}
rotation: {
w: 1
}
}
}
}
child_to_parent_edge_map {
key: "body"
value: {
parent_frame_name: ""
}
}
"""
frame_tree = _create_snapshot(snapshot_text)
assert frame_helpers.validate_frame_tree_snapshot(frame_tree)
test_vel1 = math_helpers.SE3Velocity(1.1, 2.2, 3.3, 4.4, 5.5, 6.6)
assert type(test_vel1.linear_velocity_x) == float
assert test_vel1.linear_velocity_x == 1.1
assert test_vel1.linear.x == 1.1
test_vel2 = math_helpers.SE3Velocity(1.1, 2.2, 3.3, 4.4, 5.5, 6.6)
test_vel2_proto = test_vel2.to_proto()
body_vel = frame_helpers.express_se3_velocity_in_new_frame(
frame_tree, "body", "vision", test_vel2)
assert body_vel is not None
assert type(body_vel.linear.x) == float
assert type(body_vel.linear_velocity_x) == float
assert body_vel.linear_velocity_x == 56.1
assert body_vel.linear.x == 56.1
body_vel = frame_helpers.express_se3_velocity_in_new_frame(
frame_tree, "body", "vision", test_vel2_proto)
assert body_vel is not None
assert type(body_vel.linear.x) == float
assert type(body_vel.linear_velocity_x) == float
assert body_vel.linear_velocity_x == 56.1
assert body_vel.linear.x == 56.1
test_vel3 = math_helpers.SE2Velocity(1.1, 2.2, 3.3)
test_vel3_proto = test_vel3.to_proto()
body_vel = frame_helpers.express_se2_velocity_in_new_frame(
frame_tree, "body", "vision", test_vel3)
assert body_vel is not None
assert type(body_vel.linear.x) == float
assert type(body_vel.linear_velocity_x) == float
assert body_vel.linear_velocity_x == 1.1
assert body_vel.linear.x == 1.1
body_vel = frame_helpers.express_se2_velocity_in_new_frame(
frame_tree, "body", "vision", test_vel3_proto)
assert body_vel is not None
assert type(body_vel.linear.x) == float
assert type(body_vel.linear_velocity_x) == float
assert body_vel.linear_velocity_x == 1.1
assert body_vel.linear.x == 1.1
def test_express_velocity_new_frame():
snapshot_text = """
child_to_parent_edge_map {
key: "vision"
value: {
parent_frame_name: "body"
parent_tform_child: {
position: {
x: 1
z: 10
}
rotation: {
w: 1
}
}
}
}
child_to_parent_edge_map {
key: "odom"
value: {
parent_frame_name: "vision"
parent_tform_child: {
position: {
x: 2
z: 10
}
rotation: {
w: 1
}
}
}
}
child_to_parent_edge_map {
key: "special"
value: {
parent_frame_name: "body"
parent_tform_child: {
position: {
x: 3
z: 10
}
rotation: {
w: 1
}
}
}
}
child_to_parent_edge_map {
key: "fiducial_404"
value: {
parent_frame_name: "vision"
parent_tform_child: {
position: {
x: 4
z: 0
}
rotation: {
w: 1
}
}
}
}
child_to_parent_edge_map {
key: "body"
value: {
parent_frame_name: ""
}
}
"""
frame_tree = _create_snapshot(snapshot_text)
assert frame_helpers.validate_frame_tree_snapshot(frame_tree)
# Transform SE(2) velocity
vel_of_body_in_vision = math_helpers.SE2Velocity(1, 1, 2)
vel_of_body_in_odom = frame_helpers.express_se2_velocity_in_new_frame(
frame_tree, "vision", "odom", vel_of_body_in_vision)
assert vel_of_body_in_odom is not None
assert type(vel_of_body_in_vision) == math_helpers.SE2Velocity
assert math.fabs(vel_of_body_in_odom.angular - 2) < 1e-6
assert math.fabs(vel_of_body_in_odom.linear.x - 1) < 1e-6
assert math.fabs(vel_of_body_in_odom.linear.y - 5) < 1e-6
# Transform SE(3) velocity
vel_of_body_in_vision = math_helpers.SE3Velocity(1, 2, 3, 1, 2, 3)
vel_of_body_in_odom = frame_helpers.express_se3_velocity_in_new_frame(
frame_tree, "vision", "odom", vel_of_body_in_vision)
assert vel_of_body_in_odom is not None
assert type(vel_of_body_in_vision) == math_helpers.SE3Velocity
assert math.fabs(vel_of_body_in_odom.angular.x - 1) < 1e-6
assert math.fabs(vel_of_body_in_odom.angular.y - 2) < 1e-6
assert math.fabs(vel_of_body_in_odom.angular.z - 3) < 1e-6
assert math.fabs(vel_of_body_in_odom.linear.x - 21) < 1e-6
assert math.fabs(vel_of_body_in_odom.linear.y - (-2)) < 1e-6
assert math.fabs(vel_of_body_in_odom.linear.z - (-1)) < 1e-6
def test_get_a_tform_b_se2():
snapshot_text = """
child_to_parent_edge_map {
key: "vision"
value: {
parent_frame_name: "body"
parent_tform_child: {
position: {
x: 1
y: 0
z: 10
}
rotation: {
w: 1
x: 0
y: 0
z: 0
}
}
}
}
child_to_parent_edge_map {
key: "special"
value: {
parent_frame_name: "body"
parent_tform_child: {
position: {
x: 3
y: 0
z: 10
}
rotation: {
w: 1
x: 0
y: 0
z: 0
}
}
}
}
child_to_parent_edge_map {
key: "fiducial_404"
value: {
parent_frame_name: "vision"
parent_tform_child: {
position: {
x: 4
y: 0
z: 0
}
rotation: {
w: 1
x: 0
y: 0
z: 0
}
}
}
}
child_to_parent_edge_map {
key: "body"
value: {
parent_frame_name: ""
}
}
"""
frame_tree = _create_snapshot(snapshot_text)
assert frame_helpers.validate_frame_tree_snapshot(frame_tree)
# Check that a non gravity aligned frame gets rejected.
special_tform_body = frame_helpers.get_se2_a_tform_b(
frame_tree, "special", "body")
assert special_tform_body is None
# Check that a non-existent (gravity aligned) frame is rejected
odom_tform_body = frame_helpers.get_se2_a_tform_b(frame_tree, "odom",
"body")
assert odom_tform_body is None
# Check that a gravity aligned frame is used and properly computed.
vision_tform_fiducial_404 = frame_helpers.get_se2_a_tform_b(
frame_tree, "vision", "fiducial_404")
assert vision_tform_fiducial_404 is not None
assert math.fabs(vision_tform_fiducial_404.position.x - 4) < 1e-6
assert math.fabs(vision_tform_fiducial_404.position.y) < 1e-6
assert math.fabs(vision_tform_fiducial_404.angle) < 1e-6
def test_frame_tree_math_big_tree():
snapshot_text = """child_to_parent_edge_map {
key: "beta"
value: {
parent_frame_name: "alpha"
parent_tform_child: {
position: {
x: 10
y: 0
z: 0
}
}
}
}
child_to_parent_edge_map {
key: "gamma"
value: {
parent_frame_name: "alpha"
parent_tform_child: {
position: {
x: 0
y: 0
z: 10
}
}
}
}
child_to_parent_edge_map {
key: "delta"
value: {
parent_frame_name: "beta"
parent_tform_child: {
position: {
x: 100
y: 0
z: 0
}
}
}
}
child_to_parent_edge_map {
key: "epsilon"
value: {
parent_frame_name: "beta"
parent_tform_child: {
position: {
x: 1000
y: 0
z: 0
}
}
}
}
child_to_parent_edge_map {
key: "zeta"
value: {
parent_frame_name: "gamma"
parent_tform_child: {
position: {
x: 0
y: 0
z: 100
}
}
}
}
child_to_parent_edge_map {
key: "eta"
value: {
parent_frame_name: "gamma"
parent_tform_child: {
position: {
x: 0
y: 0
z: 1000
}
}
}
}
child_to_parent_edge_map {
key: "alpha"
value: {
parent_frame_name: ""
}
}"""
frame_tree = _create_snapshot(snapshot_text)
assert frame_helpers.validate_frame_tree_snapshot(frame_tree)
# alpha as source frame
assert _do_poses_match(
0, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'alpha', 'alpha'))
assert _do_poses_match(
10, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'alpha', 'beta'))
assert _do_poses_match(
0, 0, 10, frame_helpers.get_a_tform_b(frame_tree, 'alpha', 'gamma'))
assert _do_poses_match(
110, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'alpha', 'delta'))
assert _do_poses_match(
1010, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'alpha',
'epsilon'))
assert _do_poses_match(
0, 0, 110, frame_helpers.get_a_tform_b(frame_tree, 'alpha', 'zeta'))
assert _do_poses_match(
0, 0, 1010, frame_helpers.get_a_tform_b(frame_tree, 'alpha', 'eta'))
# beta as source frame
assert _do_poses_match(
-10, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'beta', 'alpha'))
assert _do_poses_match(
0, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'beta', 'beta'))
assert _do_poses_match(
-10, 0, 10, frame_helpers.get_a_tform_b(frame_tree, 'beta', 'gamma'))
assert _do_poses_match(
100, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'beta', 'delta'))
assert _do_poses_match(
1000, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'beta', 'epsilon'))
assert _do_poses_match(
-10, 0, 110, frame_helpers.get_a_tform_b(frame_tree, 'beta', 'zeta'))
assert _do_poses_match(
-10, 0, 1010, frame_helpers.get_a_tform_b(frame_tree, 'beta', 'eta'))
# gamma as source frame
assert _do_poses_match(
0, 0, -10, frame_helpers.get_a_tform_b(frame_tree, 'gamma', 'alpha'))
assert _do_poses_match(
10, 0, -10, frame_helpers.get_a_tform_b(frame_tree, 'gamma', 'beta'))
assert _do_poses_match(
0, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'gamma', 'gamma'))
assert _do_poses_match(
110, 0, -10, frame_helpers.get_a_tform_b(frame_tree, 'gamma', 'delta'))
assert _do_poses_match(
1010, 0, -10,
frame_helpers.get_a_tform_b(frame_tree, 'gamma', 'epsilon'))
assert _do_poses_match(
0, 0, 100, frame_helpers.get_a_tform_b(frame_tree, 'gamma', 'zeta'))
assert _do_poses_match(
0, 0, 1000, frame_helpers.get_a_tform_b(frame_tree, 'gamma', 'eta'))
# delta as source frame
assert _do_poses_match(
-110, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'delta', 'alpha'))
assert _do_poses_match(
-100, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'delta', 'beta'))
assert _do_poses_match(
-110, 0, 10, frame_helpers.get_a_tform_b(frame_tree, 'delta', 'gamma'))
assert _do_poses_match(
0, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'delta', 'delta'))
assert _do_poses_match(
900, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'delta', 'epsilon'))
assert _do_poses_match(
-110, 0, 110, frame_helpers.get_a_tform_b(frame_tree, 'delta', 'zeta'))
assert _do_poses_match(
-110, 0, 1010, frame_helpers.get_a_tform_b(frame_tree, 'delta', 'eta'))
# epsilon as source frame
assert _do_poses_match(
-1010, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'epsilon',
'alpha'))
assert _do_poses_match(
-1000, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'epsilon',
'beta'))
assert _do_poses_match(
-1010, 0, 10,
frame_helpers.get_a_tform_b(frame_tree, 'epsilon', 'gamma'))
assert _do_poses_match(
-900, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'epsilon',
'delta'))
assert _do_poses_match(
0, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'epsilon', 'epsilon'))
assert _do_poses_match(
-1010, 0, 110,
frame_helpers.get_a_tform_b(frame_tree, 'epsilon', 'zeta'))
assert _do_poses_match(
-1010, 0, 1010,
frame_helpers.get_a_tform_b(frame_tree, 'epsilon', 'eta'))
# zeta as source frame
assert _do_poses_match(
0, 0, -110, frame_helpers.get_a_tform_b(frame_tree, 'zeta', 'alpha'))
assert _do_poses_match(
10, 0, -110, frame_helpers.get_a_tform_b(frame_tree, 'zeta', 'beta'))
assert _do_poses_match(
0, 0, -100, frame_helpers.get_a_tform_b(frame_tree, 'zeta', 'gamma'))
assert _do_poses_match(
110, 0, -110, frame_helpers.get_a_tform_b(frame_tree, 'zeta', 'delta'))
assert _do_poses_match(
1010, 0, -110,
frame_helpers.get_a_tform_b(frame_tree, 'zeta', 'epsilon'))
assert _do_poses_match(
0, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'zeta', 'zeta'))
assert _do_poses_match(
0, 0, 900, frame_helpers.get_a_tform_b(frame_tree, 'zeta', 'eta'))
# eta as source frame
assert _do_poses_match(
0, 0, -1010, frame_helpers.get_a_tform_b(frame_tree, 'eta', 'alpha'))
assert _do_poses_match(
10, 0, -1010, frame_helpers.get_a_tform_b(frame_tree, 'eta', 'beta'))
assert _do_poses_match(
0, 0, -1000, frame_helpers.get_a_tform_b(frame_tree, 'eta', 'gamma'))
assert _do_poses_match(
110, 0, -1010, frame_helpers.get_a_tform_b(frame_tree, 'eta', 'delta'))
assert _do_poses_match(
1010, 0, -1010,
frame_helpers.get_a_tform_b(frame_tree, 'eta', 'epsilon'))
assert _do_poses_match(
0, 0, -900, frame_helpers.get_a_tform_b(frame_tree, 'eta', 'zeta'))
assert _do_poses_match(
0, 0, 0, frame_helpers.get_a_tform_b(frame_tree, 'eta', 'eta'))
