def generate_ros_msg(self):
msg = Config()
b = BoolParameter()
b.name = "antiwindup"
b.value = False
msg.bools = [b]
p = DoubleParameter()
p.name = "p"
p.value = self.p
i = DoubleParameter()
i.name = "i"
i.value = self.i
d = DoubleParameter()
d.name = "d"
d.value = self.d
msg.doubles = [p, i, d]
g = GroupState()
g.name = "Default"
g.state = True
g.id = 0
g.parent = 0
msg.groups = [g]
return msg
def run(self, parameters):
p1 = BoolParameter(name='associator_forget_unseen', value=True)
p2 = DoubleParameter(name='cluster_tolerance_m', value=0.3)
p3 = DoubleParameter(name='associator_max_distance', value=0.25)
p4 = IntParameter(name='cluster_min_points', value=1)
p5 = IntParameter(name='persistant_cloud_filter_min_neighbors',
value=1)
yield self.pcodar_set_params(bools=[p1],
doubles=[p2, p3],
ints=[p4, p5])
# TODO: use PCODAR amnesia to avoid this timing fiasco
yield self.wait_for_task_such_that(
lambda task: task.state in ['running'])
yield self.set_vrx_classifier_enabled(SetBoolRequest(data=True))
objects = {}
while True:
task_info = yield self.task_info_sub.get_next_message()
new_objects, positions = yield self.get_object_map()
position_enu = (yield self.tx_pose)[0]
for key in new_objects:
if key not in objects:
self.send_feedback('NEW object {} {}'.format(
key, new_objects[key]))
yield self.announce_object(key, new_objects[key],
positions[key], position_enu)
elif objects[key] != new_objects[key]:
self.send_feedback('{} changed from {} to {}'.format(
key, objects[key], new_objects[key]))
yield self.announce_object(key, new_objects[key],
positions[key], position_enu)
objects = new_objects
def encode_config(config, flat=True):
msg = ConfigMsg()
for k, v in config.items():
## @todo add more checks here?
if type(v) == int:
msg.ints.append(IntParameter(k, v))
elif type(v) == bool:
msg.bools.append(BoolParameter(k, v))
elif type(v) == str:
msg.strs.append(StrParameter(k, v))
elif sys.version_info.major < 3 and isinstance(v, unicode):
msg.strs.append(StrParameter(k, v))
elif type(v) == float:
msg.doubles.append(DoubleParameter(k, v))
elif type(v) == dict or isinstance(v, Config):
if flat is True:
def flatten(g):
groups = []
for _name, group in g['groups'].items():
groups.extend(flatten(group))
groups.append(GroupState(group['name'], group['state'], group['id'], group['parent']))
return groups
msg.groups.append(GroupState(v['name'], v['state'], v['id'], v['parent']))
msg.groups.extend(flatten(v))
else:
msg.groups = [GroupState(x['name'], x['state'], x['id'], x['parent']) for x in v]
return msg
def deserializeFromYaml(yaml_str):
cfg = yaml.load(yaml_str)
if not isinstance(cfg, dict):
return None
msg = Config()
if 'bools' in cfg and isinstance(cfg['bools'], list):
for p in cfg['bools']:
try:
msg.bools.append(BoolParameter(p['name'], p['value']))
except Exception:
pass
if 'ints' in cfg and isinstance(cfg['ints'], list):
for p in cfg['ints']:
try:
msg.ints.append(IntParameter(p['name'], p['value']))
except Exception:
pass
if 'strs' in cfg and isinstance(cfg['strs'], list):
for p in cfg['strs']:
try:
msg.strs.append(StrParameter(p['name'], p['value']))
except Exception:
pass
if 'doubles' in cfg and isinstance(cfg['doubles'], list):
for p in cfg['doubles']:
try:
msg.doubles.append(DoubleParameter(p['name'], p['value']))
except Exception:
pass
return msg