def test_base_multiple_entries(self):
dummy_id_1 = "dummy1"
e1 = Entity(dummy_id_1, "dummy_type", "/map", None, None, None, None,
None)
entity1 = Designator(e1, name="entity designator")
area_name1 = "area1"
dummy_id_2 = "dummy2"
e2 = Entity(dummy_id_2, "dummy_type", "/map", None, None, None, None,
None)
entity2 = Designator(e2, name="entity designator")
area_name2 = "area2"
pc, oc = symbolic_constraint(self.robot, {
entity1: area_name1,
entity2: area_name2
})
# symbolic constraint should only generate a positionconstraint
self.assertIsNotNone(pc)
self.assertIsNone(oc)
self.robot.parts[
"ed"].navigation.get_position_constraint.assert_called_with({
dummy_id_1:
area_name1,
dummy_id_2:
area_name2
})
def make_legend():
legend = Digraph('cluster_1')
state = smach.State()
parent_desig = Designator(
name="parent_designator",
resolve_type=[str]) # No resolve_type really needed but required
desig = Designator(
name="designator",
resolve_type=[str]) # No resolve_type really needed but required
vardesig = VariableDesignator(resolve_type=str, name="variable").writeable
used_in_state = DesignatorUsedInState(state, desig, "role_resolved_from")
written_in_state = DesignatorWrittenInState(state, vardesig,
"role_written_to")
used_in_desig = DesignatorUsedInDesignator(parent_desig, desig,
"role_in_parent")
usages = [used_in_state, written_in_state, used_in_desig]
for usage in usages:
usage.add_graphviz_edge(legend)
legend.body.append('label = "Legend"')
legend.body.append('color=black')
legend.body.append('width=0.25')
legend.body.append('height=0.25')
legend.body.append('size="2.0, 2.0"')
return legend
def test_no_look(self):
# parameters
x_coordinate = 2.1
y_coordinate = 3.7
z_coordinate = 0
frame_id = "/map"
robot = Mockbot()
arm = ArmDesignator(robot, {}, name="arm_designator")
frame = Designator(frame_stamped(frame_id, x_coordinate, y_coordinate, z_coordinate), name="frame_designator")
pc, oc = arms_reach_constraint(frame, arm, look=False)
# verify positionconstraint
self.assertEqual(pc.frame, frame_id)
publicarm = arm.resolve()
radius = math.hypot(publicarm.base_offset.x(), publicarm.base_offset.y())
margin = 0.075
ro = "(x-{})^2+(y-{})^2 < {}^2".format(x_coordinate, y_coordinate, radius + margin)
ri = "(x-{})^2+(y-{})^2 > {}^2".format(x_coordinate, y_coordinate, radius - margin)
verification_string = ri + " and " + ro
equal, msg = constraint_strings_equal(pc.constraint, verification_string)
self.assertTrue(equal, msg)
# verify orientationconstraint
self.assertIsNone(oc)
def test_no_look(self):
# parameters
x_coordinate = 2.1
y_coordinate = 3.7
z_coordinate = 0
radius = 0.3
yaw = 1.57
frame_id = "/map"
pose = kdl.Frame(kdl.Rotation.RPY(0, 0, yaw),
kdl.Vector(x_coordinate, y_coordinate, z_coordinate))
e = Entity("dummy", "dummy_type", frame_id, pose, None, None, None,
None)
entity = Designator(e, name="entity designator")
pc, oc = waypoint_constraint(entity, radius, look=False)
# verify positionconstraint
self.assertEqual(pc.frame, frame_id)
verification_string = "(x-{})^2+(y-{})^2 < {}^2".format(
x_coordinate, y_coordinate, radius)
equal, msg = constraint_strings_equal(pc.constraint,
verification_string)
self.assertTrue(equal, msg)
# verify orientationconstraint
self.assertIsNone(oc)
def test_no_shape(self):
# parameters
x_coordinate = 2.1
y_coordinate = 3.7
z_coordinate = 0
radius = 0.7
margin = 0.3
frame_id = "/map"
pose = kdl.Frame(kdl.Rotation.RPY(0, 0, 0),
kdl.Vector(x_coordinate, y_coordinate, z_coordinate))
e = Entity("dummy", "dummy_type", frame_id, pose, Shape(), None, None,
None)
entity = Designator(e, name="entity designator")
pc, oc = radius_constraint(entity, radius, margin)
# verify positionconstraint
self.assertEqual(pc.frame, frame_id)
ro = "(x-{})^2+(y-{})^2 < {}^2".format(x_coordinate, y_coordinate,
radius + margin)
ri = "(x-{})^2+(y-{})^2 > {}^2".format(x_coordinate, y_coordinate,
radius - margin)
verification_string = ri + " and " + ro
equal, msg = constraint_strings_equal(pc.constraint,
verification_string)
self.assertTrue(equal, msg)
# verify orientationconstraint
self.assertIsNone(oc)
def test_offset(self):
# parameters
x_coordinate = 2.1
y_coordinate = 3.7
z_coordinate = 0
frame_id = "/map"
offset = 1.57
pose = kdl.Frame(kdl.Rotation.RPY(0, 0, 0),
kdl.Vector(x_coordinate, y_coordinate, z_coordinate))
e = Entity("dummy", "dummy_type", frame_id, pose, None, None, None,
None)
entity = Designator(e, name="entity designator")
pc, oc = look_at_constraint(entity, offset)
# verify positionconstraint
self.assertIsNone(pc)
# verify orientationconstraint
self.assertEqual(oc.look_at.x, x_coordinate)
self.assertEqual(oc.look_at.y, y_coordinate)
# The z coordinate is irrelevant in the orientation constraint
self.assertEqual(oc.angle_offset, offset)
self.assertEqual(oc.frame, frame_id)
def test_room(self):
dummy_id = "dummy_room"
r = Entity(dummy_id, "dummy_type", "/map", None, None, None, None,
None)
room = Designator(r, name="room designator")
pc, oc = room_constraint(self.robot, room)
# room constraint should only generate a positionconstraint
self.assertIsNotNone(pc)
self.assertIsNone(oc)
self.robot.parts[
"ed"].navigation.get_position_constraint.assert_called_with(
{dummy_id: "in"})
def test_base(self):
dummy_id = "dummy"
e = Entity(dummy_id, "dummy_type", "/map", None, None, None, None,
None)
entity = Designator(e, name="entity designator")
area_name = "area"
pc, oc = symbolic_constraint(self.robot, {entity: area_name})
# symbolic constraint should only generate a positionconstraint
self.assertIsNotNone(pc)
self.assertIsNone(oc)
self.robot.parts[
"ed"].navigation.get_position_constraint.assert_called_with(
{dummy_id: area_name})
def test_visited_and_unreachable(self):
"""In our RoboCup executives, we keep track of which items are 'processed' or visited,
and which cannot be processed because they are unreachable.
One way to do this, is to mark objectIDs (targets) as visited and unreachable in the reasoner,
e.g. a global blackboard that can be manipulated an queried through Prolog.
This gets messy with complex queries. With designators, it should be a lot simpler"""
success = lambda goal: goal % 2 # Even targets will fail
target_list = [i for i in range(10)] + [i for i in range(10)
] # all are in twice!
targets = Designator(target_list)
unreachable_set = set()
unreachable = VariableDesignator(unreachable_set, resolve_type=set)
visited_set = set()
visited = VariableDesignator(visited_set, resolve_type=set)
for target in target_list:
if target not in unreachable.resolve(
) and target not in visited.resolve():
print("Processing target {0}".format(target))
if success(target):
visited.resolve().add(
target) # This is a set so no += [...]
print("Target {0} was successfull, its visited".format(
target))
else:
unreachable.resolve().add(
target) # This is a set so no += [...]
print("Target {0} failed, its unreachable".format(target))
else:
print(
"NOT processing target {0}: its unreachable or already visited"
.format(target))
print("##### Done #####")
print("These are unreachable: {0}".format(unreachable.resolve()))
print("These are visited: {0}".format(visited.resolve()))
self.assertEqual(len(unreachable.resolve()), 5)
self.assertEqual(len(visited.resolve()), 5)
def test_list(self):
v = Designator(['a', 'b', 'c'])
self.assertEqual(v.resolve_type, [str])
self.assertListEqual(v.resolve(), ['a', 'b', 'c'])
import PyKDL as kdl
# Robot Smach States
from robot_smach_states.navigation.constraint_functions.look_at_constraints import look_at_constraint
from robot_smach_states.util.designators.core import Designator
if __name__ == "__main__":
"""
Example demonstrating how to use the look_at_constraints function
"""
parser = argparse.ArgumentParser(description="Example look_at_constaints function")
args = parser.parse_args()
rospy.init_node("example_look_at_constaints")
# get an entitydesignator
pose = kdl.Frame(kdl.Rotation.RPY(0, 0, 0), kdl.Vector(1, 2, 1.3))
e = Entity("dummy", "dummy_type", "/map", pose, None, None, None, None)
entity = Designator(e, name="entity designator")
# example base function
rospy.loginfo("look at an entity")
pc, oc = look_at_constraint(entity)
rospy.loginfo("pc becomes : {}".format(pc))
rospy.loginfo("oc becomes : {}".format(oc))
rospy.loginfo("orient yourself w.r.t an entity")
pc, oc = look_at_constraint(entity, offset=1.57) # keep the object 90 degrees to your left
rospy.loginfo("pc becomes : {}".format(pc))
rospy.loginfo("oc becomes : {}".format(oc))
"""
parser = argparse.ArgumentParser(
description="Example arms_reach_constaint() function")
parser.add_argument("--robot",
default="hero",
help="Robot name (amigo, hero, sergio)")
args = parser.parse_args()
rospy.init_node("example_arms_reach_constraints")
robot = get_robot(args.robot)
# get an armdesignator and a pose designator
arm_designator = ArmDesignator(robot, {}, name="arm_designator")
pose = frame_stamped("/map", 0, 0, 1.3)
pose_designator = Designator(pose, name="frame designator")
# basic functionality
rospy.loginfo("Example of the basic funtionality")
rospy.loginfo(
"Running: pc, oc = arms_reach_constraint(pose_designator, arm_designator)"
)
pc, oc = arms_reach_constraint(pose_designator, arm_designator)
rospy.loginfo("pc becomes : {}".format(pc))
rospy.loginfo("oc becomes : {}".format(oc))
# dont look at the frame
rospy.loginfo(
"Example where we don't require the robot to look at the pose")
rospy.loginfo(
"Running: pc, oc = arms_reach_constraint(pose_designator, arm_designator, look=False)"
import argparse
# Robot Smach States
from robot_smach_states.util.designators.utility import AttrDesignator
from robot_smach_states.util.designators.core import Designator
from collections import namedtuple
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="Test the atttrDesignator")
args = parser.parse_args()
rospy.init_node("test_attrDesignator")
# Example 1 from docstrings
d = Designator(object(), resolve_type=object)
#Get the __doc__ attribute of the object that d resolves to. d is an object and d.__doc__ is 'The most base type'
wrapped = AttrDesignator(d, '__doc__', resolve_type=str)
wrapped.resolve() == 'The most base type'
rospy.loginfo(issubclass(wrapped.resolve_type, str))
# Example 2 from docstrings
d2 = Designator("banana", resolve_type=str)
wrapped2 = AttrDesignator(d2, '__class__.__class__', resolve_type=type)
wrapped2.resolve()
rospy.loginfo(wrapped2.resolve())
# Example 3 from docstrings
A = namedtuple("A", ['foo'])
B = namedtuple("B", ['bar'])
from robot_smach_states.util.designators.string_manipulation import FieldOfHMIResult
from robot_smach_states.util.designators.core import Designator
if __name__ == "__main__":
parser = argparse.ArgumentParser(
description="Test the string manipulation")
args = parser.parse_args()
rospy.init_node("Set_gripper_state")
# Test 1 from docstrings
query_result = HMIResult(
sentence='ignored',
semantics={u'widget': {
u'gadget': {
u'bla': u'foo',
u'bar': u'buzz'
}
}})
query_des = Designator(query_result, name="d1")
field_des = FieldOfHMIResult(query_des,
semantics_path=['widget', 'gadget', 'bar'])
rospy.loginfo(field_des.resolve())
# Test 2 from docstrings
query_result2 = HMIResult(sentence='ignored', semantics=u'dinges')
query_des2 = Designator(query_result2, name="d2")
field_des2 = FieldOfHMIResult(query_des2, semantics_path=[])
rospy.loginfo(field_des2.resolve())
import argparse
from robot_skills.util.entity import Entity
import PyKDL as kdl
# Robot Smach States
from robot_smach_states.navigation.constraint_functions.waypoint_constraints import waypoint_constraint
from robot_smach_states.util.designators.core import Designator
if __name__ == "__main__":
"""
Example demonstrating how to use the waypoint_constraint() function
"""
parser = argparse.ArgumentParser(
description="Example waypoint_constraint() function")
args = parser.parse_args()
rospy.init_node("example_waypoint_constaints")
# get an entitydesignator
pose = kdl.Frame(kdl.Rotation.RPY(0, 0, 0), kdl.Vector(1, 2, 0))
e = Entity("dummy_waypoint", "dummy_type", "/map", pose, None, None, None,
None)
waypoint = Designator(e, name="entity designator")
# waypoint example
rospy.loginfo("waypoint_constraint resolves to:")
pc, oc = waypoint_constraint(waypoint, radius=1.0)
rospy.loginfo("pc becomes : {}".format(pc))
rospy.loginfo("oc becomes : {}".format(oc))
def test_resolve_type(self):
d1 = Designator("some string", name="tester1", resolve_type=str)
result = d1.resolve()
self.assertEqual(result, "some string")
def test_wrong_resolve_type(self):
d2 = Designator("not an integer", name="tester2", resolve_type=int)
with self.assertRaises(TypeError):
d2.resolve()
self.assertTrue(issubclass(d2.resolve_type, int))