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 _create_parameter_updates_message(self, camera_position): """ Create a parameter_updates message to update the deprecated dynamic_reconigurable pan, tilt and zoom params. :param camera_position: The camera position. Should contain keys 'pan', 'tilt', and probably also 'zoom'. :type camera_position: dict :return: The Config message. :rtype: :py:class:`axis_camera.cfg.Config` """ message = Config() message.doubles.append( DoubleParameter(name="pan", value=camera_position['pan'])) message.doubles.append( DoubleParameter(name="tilt", value=camera_position['tilt'])) message.ints.append( IntParameter(name="zoom", value=camera_position['zoom'])) return message
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