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