def test_get_symbol_map(self, keys_values):
keys, values = keys_values
gm = GodMap()
for key, value in zip(keys, values):
gm.safe_set_data([key], value)
gm.to_symbol([key])
self.assertEqual(len(gm.get_values(keys)), len(keys))
def test_god_map_with_world(self):
gm = GodMap()
w = World()
r = WorldObject(pr2_urdf())
w.add_robot(r, PoseStamped(), [], 0, KeyDefaultDict(lambda key: 0),
False)
gm.safe_set_data([u'world'], w)
assert r == gm.safe_get_data([u'world', u'robot'])
def test_list_double_index(self):
key = 'asdf'
value = np.array([[0, 1], [2, 3]])
db = GodMap()
db.safe_set_data([key], value)
for i in range(value.shape[0]):
for j in range(value.shape[1]):
self.assertEqual(db.safe_get_data([key, i, j]), value[i, j])
def test_dict2(self, key, key_values):
d = {}
db = GodMap()
db.safe_set_data([key], d)
for k, v in zip(*key_values):
db.safe_set_data([key, k], v)
for k, v in zip(*key_values):
self.assertEqual(db.safe_get_data([key, k]), v)
def test_class3(self, class_names):
db = GodMap()
for i, name in enumerate(class_names):
c = type(str(name), (object, ), {})()
db.safe_set_data(class_names[:i + 1], c)
for i, name in enumerate(class_names):
c = db.safe_get_data(class_names[:i + 1])
self.assertEqual(type(c).__name__, name)
def test_list_overwrite_entry(self, key, lists):
first_values, second_values = lists
db = GodMap()
db.safe_set_data([key], first_values)
for i, v in enumerate(first_values):
self.assertEqual(db.safe_get_data([key, i]), v)
db.safe_set_data([key, i], second_values[i])
for i, v in enumerate(second_values):
self.assertEqual(db.safe_get_data([key, i]), v)
def test_function4(self, key1, key2, key3, key4, key5):
db = GodMap()
class MUH(object):
def __call__(self, next_member, next_next_member):
return [key5]
a = MUH()
d = {key2: a}
db.safe_set_data([key1], d)
self.assertEqual(key5, db.safe_get_data([key1, key2, (key3, key4), 0]))
def test_function_2param_call(self, key1, key2, key3, key4):
db = GodMap()
class MUH(object):
def __call__(self, next_member, next_next_member):
return next_next_member
a = MUH()
d = {key2: a}
db.safe_set_data([key1], d)
self.assertEqual(db.safe_get_data([key1, key2, (key3, key4)]), key4)
def test_function_no_param(self, key1, key2, key3):
db = GodMap()
class MUH(object):
def __call__(self):
return [key3]
a = MUH()
d = {key2: a}
db.safe_set_data([key1], d)
self.assertEqual(key3, db.safe_get_data([key1, key2, [], 0]))
def test_class1(self, key, class_name, key_values):
c = type(str(class_name), (object, ), {})()
for k, v in zip(*key_values):
setattr(c, k, None)
db = GodMap()
db.safe_set_data([key], c)
for k, v in zip(*key_values):
self.assertEqual(db.safe_get_data([key, k]), None)
for k, v in zip(*key_values):
db.safe_set_data([key, k], v)
for k, v in zip(*key_values):
self.assertEqual(db.safe_get_data([key, k]), v)
def test_function3(self, key1, key2, key3, key4, key5):
db = GodMap()
class MUH(object):
def __call__(self, next_member):
return [key5]
a = MUH()
d = {key2: a}
db.safe_set_data([key1], d)
try:
db.safe_get_data([key1, key2, (key3, key4), 0])
assert False
except TypeError:
assert True
def test_god_map_with_world(self):
gm = GodMap()
w = World()
r = WorldObject(pr2_urdf())
w.add_robot(robot=r,
base_pose=PoseStamped(),
controlled_joints=[],
joint_vel_limit=KeyDefaultDict(lambda key: 0),
joint_acc_limit=KeyDefaultDict(lambda key: 0),
joint_weights=KeyDefaultDict(lambda key: 0),
calc_self_collision_matrix=False,
ignored_pairs=set(),
added_pairs=set(),
symengine_backend='llvm',
symengine_opt_level=0)
gm.safe_set_data([u'world'], w)
gm_robot = gm.safe_get_data(identifier.robot)
assert 'pr2' == gm_robot.get_name()
def test_namedtuple1(self):
Frame = namedtuple(u'Frame', [u'pos'])
db = GodMap()
db.safe_set_data([u'f12'], Frame(pos=2))
with self.assertRaises(AttributeError):
db.safe_set_data([u'f12', u'pos'], 42)
def test_namedtuple(self, key, tuple_name, key_values):
Frame = namedtuple(tuple_name, key_values[0])
db = GodMap()
db.safe_set_data([key], Frame(*key_values[1]))
for k, v in zip(*key_values):
self.assertEqual(db.safe_get_data([key, k]), v)
def test_set_get_float(self, key, number):
db = GodMap()
db.safe_set_data([key], number)
db.safe_set_data([key], number)
self.assertEqual(db.safe_get_data([key]), number)
def test_dict3(self, key, tuple_key, value):
tuple_key = tuple(tuple_key)
d = {tuple_key: value}
db = GodMap()
db.safe_set_data([key], d)
self.assertEqual(db.safe_get_data([key, tuple_key]), value)
def test_set_get_integer(self, key, number):
db = GodMap()
db.safe_set_data([key], number)
self.assertEqual(db.safe_get_data([key]),
number,
msg=u'key={}, number={}'.format(key, number))
def test_to_symbol(self, key, value):
gm = GodMap()
gm.safe_set_data([key], value)
self.assertTrue(w.is_symbol(gm.to_symbol([key])))
self.assertTrue(key in str(gm.to_symbol([key])))
def test_tuple1(self, key, value):
value = tuple(value)
db = GodMap()
db.safe_set_data([key], value)
for i, v in enumerate(value):
self.assertEqual(db.safe_get_data([key, i]), v)
def test_list_index_error(self, key, l):
db = GodMap()
db.safe_set_data([key], l)
with self.assertRaises(IndexError):
db.safe_set_data([key, len(l) + 1], 0)
def test_function_1param_lambda(self, key, key2):
db = GodMap()
f = lambda x: x
db.safe_set_data([key], f)
self.assertEqual(db.safe_get_data([key, [key2]]), key2)
def test_list_negative_index(self, key, l):
db = GodMap()
db.safe_set_data([key], l)
for i in range(len(l)):
self.assertEqual(db.safe_get_data([key, -i]), l[-i])
def initialize_god_map():
god_map = GodMap()
blackboard = Blackboard
blackboard.god_map = god_map
god_map.safe_set_data(identifier.rosparam, rospy.get_param(rospy.get_name()))
god_map.safe_set_data(identifier.robot_description, rospy.get_param(u'robot_description'))
path_to_data_folder = god_map.safe_get_data(identifier.data_folder)
# fix path to data folder
if not path_to_data_folder.endswith(u'/'):
path_to_data_folder += u'/'
god_map.safe_set_data(identifier.data_folder, path_to_data_folder)
# fix nWSR
nWSR = god_map.safe_get_data(identifier.nWSR)
if nWSR == u'None':
nWSR = None
god_map.safe_set_data(identifier.nWSR, nWSR)
pbw.start_pybullet(god_map.safe_get_data(identifier.gui))
while not rospy.is_shutdown():
try:
controlled_joints = rospy.wait_for_message(u'/whole_body_controller/state',
JointTrajectoryControllerState,
timeout=5.0).joint_names
except ROSException as e:
logging.logerr(u'state topic not available')
logging.logerr(e)
else:
break
rospy.sleep(0.5)
joint_weight_symbols = process_joint_specific_params(identifier.joint_weights,
identifier.default_joint_weight_identifier, god_map)
process_joint_specific_params(identifier.collisions_distances, identifier.default_collision_distances, god_map)
joint_vel_symbols = process_joint_specific_params(identifier.joint_vel, identifier.default_joint_vel, god_map)
joint_acc_symbols = process_joint_specific_params(identifier.joint_acc, identifier.default_joint_acc, god_map)
world = PyBulletWorld(god_map.safe_get_data(identifier.gui),
blackboard.god_map.safe_get_data(identifier.data_folder))
god_map.safe_set_data(identifier.world, world)
robot = WorldObject(god_map.safe_get_data(identifier.robot_description),
None,
controlled_joints)
world.add_robot(robot, None, controlled_joints, joint_vel_symbols, joint_acc_symbols, joint_weight_symbols, True,
ignored_pairs=god_map.safe_get_data(identifier.ignored_self_collisions),
added_pairs=god_map.safe_get_data(identifier.added_self_collisions),
symengine_backend=god_map.safe_get_data(identifier.symengine_backend),
symengine_opt_level=god_map.safe_get_data(identifier.symengine_opt_level))
joint_position_symbols = JointStatesInput(blackboard.god_map.to_symbol, world.robot.get_controllable_joints(),
identifier.joint_states,
suffix=[u'position'])
joint_vel_symbols = JointStatesInput(blackboard.god_map.to_symbol, world.robot.get_controllable_joints(),
identifier.joint_states,
suffix=[u'velocity'])
world.robot.reinitialize(joint_position_symbols.joint_map, joint_vel_symbols.joint_map)
return god_map
def test_to_symbol(self, key, value):
gm = GodMap()
gm.safe_set_data([key], value)
self.assertTrue(isinstance(gm.to_symbol([key]), sw.Symbol))
self.assertTrue(key in str(gm.to_symbol([key])))
