def histogram_of_stop_point_elbow(self, subjects, labels): fig1 = plt.figure(4) fig2 = plt.figure(5) labels = ['good'] stop_locations = [] arm_lengths = [] for num, label in enumerate(labels): for subj_num, subject in enumerate(subjects): subject_stop_locations = [] paramlist = rosparam.load_file(''.join([data_path, '/', subject, '/params.yaml'])) for params, ns in paramlist: rosparam.upload_params(ns, params) arm_length = rosparam.get_param('crook_to_fist')/100. # fig1 = plt.figure(2*num+1) ax1 = fig1.add_subplot(111) ax1.set_xlim(0.2, .4) # ax1.set_ylim(-10.0, 1.0) ax1.set_xlabel('Stop Position (m)', fontsize=20) ax1.set_ylabel('Number of trials', fontsize=20) ax1.set_title(''.join(['Difference between arm length and stop position at the elbow']), fontsize=20) ax1.tick_params(axis='x', labelsize=20) ax1.tick_params(axis='y', labelsize=20) ax2 = fig2.add_subplot(431+subj_num) ax2.set_xlim(0.2, .4) # ax1.set_ylim(-10.0, 1.0) ax2.set_xlabel('Position (m)') ax2.set_ylabel('Number of trials') ax2.set_title(''.join(['Stop position for "Good" outcome']), fontsize=20) vel = 0.1 directory = ''.join([data_path, '/', subject, '/formatted_three/', str(vel),'mps/', label, '/']) force_file_list = os.listdir(directory) for file_name in force_file_list: # print directory+file_name loaded_data = load_pickle(directory+file_name) stop_locations.append(np.max(loaded_data[:,1])-arm_length) subject_stop_locations.append(np.max(loaded_data[:,1])-arm_length) arm_lengths.append(arm_length) vel = 0.15 directory = ''.join([data_path, '/', subject, '/formatted_three/', str(vel),'mps/', label, '/']) force_file_list = os.listdir(directory) for file_name in force_file_list: loaded_data = load_pickle(directory+file_name) stop_locations.append(np.max(loaded_data[:,1]-arm_length)) subject_stop_locations.append(np.max(loaded_data[:,1])-arm_length) ax2.hist(subject_stop_locations) mu = np.mean(stop_locations) sigma = np.std(stop_locations) print 'The minimum arm length is: ', np.min(arm_lengths) print 'The max arm length is: ', np.max(arm_lengths) print 'The mean arm length is: ', np.mean(arm_lengths) print 'The mean of the stop location is: ', mu print arm_lengths print 'The standard deviation of the stop location is: ', sigma n, bins, patches = ax1.hist(stop_locations, 10, color="green", alpha=0.75) points = np.arange(0, 10, 0.001) y = mlab.normpdf(points, mu, sigma) l = ax1.plot(points, y, 'r--', linewidth=2)
def __init__(self): self.client = actionlib.SimpleActionClient("/move_base", MoveBaseAction) self.client.wait_for_server() self.locations = rosparam.load_file( str(Path.home()) + "/catkin_ws/src/competition2/yaml/locations.yaml")[0][0] self.numbered_locations = rosparam.load_file( str(Path.home()) + "/catkin_ws/src/competition2/yaml/numbered_locations.yaml")[0][0] self.doorway_locations = rosparam.load_file( str(Path.home()) + "/catkin_ws/src/competition2/yaml/doorway_locations.yaml")[0][0] self.cmd_vel = rospy.Publisher("/cmd_vel", Twist, queue_size=10) self.twist = Twist() self.amcl_pose = rospy.Subscriber("/amcl_pose", PoseWithCovarianceStamped, self.amcl_pose_callback) self.covariance = {} # wait for covariance subscriber to receive data while len(self.covariance) != 3: pass self.initial_pose = rospy.Publisher("/initialpose", PoseWithCovarianceStamped, queue_size=10) self.move = Move()
def __init__(self, name): rospy.loginfo("Starting %s" % name) a_list = rospy.get_param("han_action_dispatcher")["han_actions"] param_file = rospy.get_param("~param_file") for a in a_list.keys(): rosparam.load_file(param_file, a, True) rospy.loginfo("done")
def fillAliasCombo(self): # Fill ComboBox with saved positions positions_dict = rosparam.load_file(ALIAS_FILE)[0][0] positions = sorted(positions_dict.keys()) for position in positions: self._widget.alias_combo.addItem(position)
def __init__(self): try: self._map = rospy.get_param("~map") except KeyError as e: rospy.logfatal("map param not set!: %s " % e) raise MapSetupException("map param not set") # Clean/delete params: try: rospy.delete_param("mmap") except: pass # Publish map_in_use (latched): self._map_in_use_pub = rospy.Publisher("map_in_use", String, queue_size=1, latch=True) self._map_in_use_pub.publish("submap_0") # Set params: rospy.set_param("map_package_in_use" , "deprecated") rospy.set_param("nice_map_in_use" , os.path.join(self._map, "map.bmp")) rospy.set_param("map_transformation_in_use", os.path.join(self._map, "transformation.xml")) # Load paramlist = rosparam.load_file(os.path.join(self._map, "mmap.yaml"), "mmap") for param, ns in paramlist: try: rosparam.upload_params(ns, param) except: pass # ignore empty params
def update(self): self.actions = [] rospack = rospkg.RosPack() packages = rospack.get_depends_on(self.name, implicit=False) for package in packages: manifest = rospack.get_manifest(package) file_path = manifest.get_export(self.name, 'actions') if not file_path: continue elif len(file_path) != 1: rospy.logwarn("Cannot load actions [%s]: invalid 'actions' attribute."%(pkg)) continue file_path = file_path[0] try: if not os.path.isfile(file_path): rospy.logwarn('Actions parameter file with path "' + file_path + '" does not exists.') continue package_path = rospack.get_path(package) parameters = load_file(file_path) for action_parameters in parameters[0][0]['actions']: entry = ActionEntry(package_path, action_parameters['action']) self.actions.append(entry) except Exception: rospy.logwarn("Unable to load actions [%s] from package [%s]."%(file_path, package)) self.actions = sorted(self.actions, key = lambda x: (x.id)) return True
def load_params(self, req): ''' EXAMPLE OF LOADING PARAMETERS FROM A YAML FILE: paramlist=rosparam.load_file("/path/to/myfile",default_namespace="my_namespace") for params, ns in paramlist: rosparam.upload_params(ns,params) ''' resp = LoadResponse() if len(req.type) == 0: resp.status.result = Status.FAILURE resp.status.error = Status.TYPE_MISSING resp.status.info = "No type provided!" else: path = join(self._root, req.type) filename = join(path, req.id) if not os.path.exists(path): resp.status.result = Status.FAILURE resp.status.error = Status.NO_SUCH_TYPE resp.status.info = "Type %s does not exist!"%(req.type) elif not os.path.exists(filename): resp.status.result = Status.FAILURE resp.status.error = Status.FILE_MISSING resp.status.info = "File %s does not exist as a member of type %s!"%(req.id, req.type) else: paramlist=rosparam.load_file(filename) for params, ns in paramlist: rosparam.upload_params(ns,params) resp.status.result = Status.SUCCESS return resp
def loadYaml(self, file, namespace): ''' Load all the parameters on the parameter server @return: True if OK, False otherwise ''' parameters = rosparam.load_file("file") rosparam.upload_params(namespace, parameters[0][0][namespace])
def load_thruster_ports(self, ports_layout, thruster_definitions): ''' Loads a dictionary ThrusterPort objects ''' self.ports = {} # ThrusterPort objects self.thruster_to_port_map = {} # node_id to ThrusterPort rospack = rospkg.RosPack() self.make_fake = rospy.get_param('simulate', False) if self.make_fake: rospy.logwarn("Running fake thrusters for simulation, based on parameter '/simulate'") # Instantiate thruster comms port for port_info in ports_layout: port_name = port_info['port'] self.ports[port_name] = thruster_comm_factory(port_info, thruster_definitions, fake=self.make_fake) # Add the thrusters to the thruster dict and configure if present for thruster_name in port_info['thruster_names']: self.thruster_to_port_map[thruster_name] = port_info['port'] if thruster_name not in self.ports[port_name].online_thruster_names: rospy.logerr("ThrusterDriver: {} IS MISSING!".format(thruster_name)) else: rospy.loginfo("ThrusterDriver: {} registered".format(thruster_name)) # Set firmware settings port = self.ports[port_name] node_id = thruster_definitions[thruster_name]['node_id'] config_path = (rospack.get_path('sub8_videoray_m5_thruster') + '/config/firmware_settings/' + thruster_name + '.yaml') rospy.loginfo('Configuring {} with settings specified in {}.'.format(thruster_name, config_path)) port.set_registers_from_dict(node_id=node_id, reg_dict=rosparam.load_file(config_path)[0][0]) port.reboot_thruster(node_id) # Necessary for some settings to take effect
def load_params(self, req): ''' EXAMPLE OF LOADING PARAMETERS FROM A YAML FILE: paramlist=rosparam.load_file("/path/to/myfile",default_namespace="my_namespace") for params, ns in paramlist: rosparam.upload_params(ns,params) ''' resp = LoadResponse() if len(req.type) == 0: resp.status.result = Status.FAILURE resp.status.error = Status.TYPE_MISSING resp.status.info = "No type provided!" else: path = join(self._root, req.type) filename = join(path, req.id) if not os.path.exists(path): resp.status.result = Status.FAILURE resp.status.error = Status.NO_SUCH_TYPE resp.status.info = "Type %s does not exist!" % (req.type) elif not os.path.exists(filename): resp.status.result = Status.FAILURE resp.status.error = Status.FILE_MISSING resp.status.info = "File %s does not exist as a member of type %s!" % ( req.id, req.type) else: paramlist = rosparam.load_file(filename) for params, ns in paramlist: rosparam.upload_params(ns, params) resp.status.result = Status.SUCCESS return resp
def __init__(self): try: self._map = rospy.get_param("~map") except KeyError as e: rospy.logfatal("map param not set!: %s " % e) raise MapSetupException("map param not set") # Clean/delete params: try: rospy.delete_param("mmap") except: pass # Publish map_in_use (latched): self._map_in_use_pub = rospy.Publisher("map_in_use", String, queue_size=1, latch=True) self._map_in_use_pub.publish("submap_0") # Set params: rospy.set_param("map_package_in_use", "deprecated") rospy.set_param("nice_map_in_use", os.path.join(self._map, "map.bmp")) rospy.set_param("map_transformation_in_use", os.path.join(self._map, "transformation.xml")) # Load paramlist = rosparam.load_file(os.path.join(self._map, "mmap.yaml"), "mmap") for param, ns in paramlist: try: rosparam.upload_params(ns, param) except: pass # ignore empty params
def init(): try: data = rosparam.load_file("smach.yaml", default_namespace="smach") print "load yaml file success" #update to server for params, ns in data: rosparam.upload_params(ns, params) time.sleep(0.5) except: print "load yaml file faile" sys.exit(0) try: slam = int(rospy.get_param('~/smach/slam')) navigation = int(rospy.get_param('~/smach/navigation')) control = int(rospy.get_param('~/smach/control')) print "load param success" time.sleep(0.5) print "**********************************************" print "value:" print("slam { %d }" % (slam)) #0:none, 1:open print("navigation { %d }" % (navigation) ) #0:none, 1:location1, 2:location2 ,etc print("control { %d }" % (control) ) #0:none, 1:front, 2:back, 3:left, 2:right print "**********************************************" time.sleep(0.1) except: print "load param faile" sys.exit(0)
def launch_platform_controls_spawner(self): rospack = rospkg.RosPack() srvss_bobcat_pkg_path = rospack.get_path('srvss_bobcat') srvss_bobcat_controllers_yaml = srvss_bobcat_pkg_path + "/controllers/srvss_bobcat_controllers.yaml" paramlist = rosparam.load_file(srvss_bobcat_controllers_yaml, default_namespace='', verbose=True) for params, ns in paramlist: rosparam.upload_params(ns, params) # if the controllers do not load it possible to increase the time of waiting for the server in the spawner # it located in /home/userws3/gz_ros_cws/src/ros_control/controller_manager/scripts node = ROSNode("controller_manager", "spawner", name="platform_controllers_spawner", namespace="/srvss_bobcat", output="screen", respawn="false", args="joint_state_controller \ front_right_wheel_velocity_controller \ front_left_wheel_velocity_controller \ back_right_wheel_velocity_controller \ back_left_wheel_velocity_controller") self.launcher.launch(node) time.sleep(10) node = ROSNode("srvss_bobcat", "srvss_bobcat_rate2effort_node", name="srvss_bobcat_RateToEffort_node", cwd="node", output="screen") self.launcher.launch(node) time.sleep(3)
def load_rois(self): """ """ import rosparam # TODO also check in current directory? #files = glob.glob('compressor_rois_*.yaml') files = glob.glob(os.path.join(rospy.get_param('source_directory'), 'compressor_rois_*.yaml')) if len(files) < 1: rospy.logfatal( 'Did not find any files matching compressor_rois_*.yaml') return [] elif len(files) > 1: rospy.logfatal( 'Found too many files matching compressor_rois_*.yaml') return [] filename = os.path.abspath(files[0]) # get the parameters in the namespace of the name we want # TODO find roi specifiers wherever they are, in the future paramlist = rosparam.load_file(filename) ns = 'delta_compressor' ns_param_dict = self.find_roi_namespace(ns, paramlist) if ns_param_dict is None: rospy.logfatal('could not find parameter namespace: ' + ns) return rois = self.find_and_call_function('load_', 'parameter dump loading', params=ns_param_dict) rospy.logwarn('loaded rois:' + str(rois)) self.launch_queue.put(rois)
def __init__(self): ''' connect to midi device and set up ros ''' pygame.midi.init() devices = pygame.midi.get_count() if devices < 1: rospy.logerr("No MIDI devices detected") exit(-1) rospy.loginfo("Found %d MIDI devices" % devices) input_dev = int(rospy.get_param("~input_dev", pygame.midi.get_default_input_id)) rospy.loginfo("Using input device %d" % input_dev) self.controller = pygame.midi.Input(input_dev) # load in default parameters if not set if not rospy.has_param('~modes'): rospack = rospkg.RosPack() paramlist = rosparam.load_file(rospack.get_path('korg_nanokontrol') + '/config/nanokontrol_config.yaml') for params, ns in paramlist: rosparam.upload_params(ns, params) self.modes = rospy.get_param('~modes') # determine how axis should be interpreted self.center_axis = rospy.get_param('~center_axis', True) self.msg = Joy() self.mode = None self.pub = rospy.Publisher('joy', Joy, queue_size=10, latch=True)
def load_device_map(self, fname): try: params = rosparam.load_file(fname)[0][0] if 'lights' in params: lights = params for k, v in params['lights'].items(): try: light_id = int(k, 16) alias = v['alias_id'] except ValueError: rospy.logerr( f'Failed to load device map config: device ID should be hexadecimal, but \'{k}\' value was found instead' ) return False except KeyError: rospy.logerr( f'Failed to load device map config: missing alias_id key' ) return False rosparam.upload_params(rospy.get_name(), lights) else: rospy.logerr( f'Failed to load device map config: unable to find \'lights\' parameter' ) return False except Exception as e: rospy.logerr(f'Failed to load device map config: {e}') return False return True
def rosparam(*args): """ Load rosparam YAML file. This can either be called with rosparam(filename) or rosparam(pkg, filename) @return: loaded data """ #TODO: should this do a search to find filename if len(args) == 2: pkg, filename = args d = roslib.packages.get_pkg_dir(pkg) matches = roslib.packages.find_resource(pkg, filename) if matches: filename = matches[0] else: filename = None elif len(args) == 1: filename = args[0] else: raise ValueError(args) if not filename: #TODO: what should the error behavior of ROSH be? return None #TODO: this strip any namespace in the file return _rosparam.load_file(filename)[0][0]
def srv_callback(self, request): label = request.label response = SaveSpotServiceMessageResponse() """ --- bool navigation_successfull string message # Direction """ os.chdir("/home/zulhafiz/catkin_ws/src/kasimx_navigation/spots") paramlist = rosparam.load_file("spots.yaml", default_namespace=None) for params, ns in paramlist: #ns,param for key, value in params.items(): if key == request.label: rosparam.upload_params(ns, params) #ns,param response.message = "Correctly uploaded parameters" send_coordinates = SendCoordinates(request.label) response.navigation_successfull = True return response
def __init__(self): # Parameters # global settings self.cfg_ = rospy.get_param('~cfg', rosparam.load_file( os.path.join(iarc_root(), 'config', 'perception.yaml'))[0][0] ) print 'cfg' print '===============' print self.cfg_ print '===============' # parameter overrides self.src_ = rospy.get_param('~src', self.cfg_['src']) self.dbg_ = rospy.get_param('~dbg', self.cfg_['dbg']) self.root_ = rospy.get_param('~root', os.path.join(iarc_root(), 'data')) self.max_dt_ = rospy.get_param('~max_dt', self.cfg_['max_dt']) self.dfac_ = DetectorFactory() # Data self.data_ = {} # ROS Handles self.cvbr_ = CvBridge() self.tf_buf_ = tf2_ros.Buffer() self.tfl_ = tf2_ros.TransformListener(self.tf_buf_) self.cam_ = {k : CameraHandle(k, self.cvbr_, self.data_cb) for k in self.src_} self.modules_ = {m : self.dfac_(m['model'], self.cfg_[m]) for m in self.cfg_['modules']}
def carefree_switch(self, arm, new_controller, param_file=None, possible_controllers=None): if '%s' in new_controller: new_controller = new_controller % arm if param_file is not None: params = rosparam.load_file( roslib.substitution_args.resolve_args(param_file)) rosparam.upload_params("", params[0][0]) if possible_controllers is None: possible_controllers = POSSIBLE_ARM_CONTROLLERS check_arm_controllers = [ controller % arm for controller in possible_controllers ] resp = self.list_controllers_srv() start_controllers, stop_controllers = [new_controller], [] for i, controller in enumerate(resp.controllers): if controller in check_arm_controllers and resp.state[ i] == 'running': stop_controllers.append(controller) self.load_controller(new_controller) rospy.loginfo("[pr2_controller_switcher] Starting controller %s" % (start_controllers[0]) + " and stopping controllers: [" + ",".join(stop_controllers) + "]") resp = self.switch_controller_srv(start_controllers, stop_controllers, 1) return resp.ok
def __init__( self, output_topic="/bounding_box_2d_monitor", rgb_topic="/hsrb/head_rgbd_sensor/rgb/image_rect_color", depth_topic="/hsrb/head_rgbd_sensor/depth_registered/image_rect_raw", camera_info_topic="/hsrb/head_rgbd_sensor/rgb/camera_info", yolo_yaml="yolov3.yaml", save_image=True, z_offset=0.1, MEMO="/spacoty/get_dataset"): super(hsr_SpaCoTyGetDataset, self).__init__(outcomes=['completed', 'failed'], input_keys=['save_folder']) # Create the action client when building the behavior. # This will cause the behavior to wait for the client before starting execution # and will trigger a timeout error if it is not available. # Using the proxy client provides asynchronous access to the result and status # and makes sure only one client is used, no matter how often this state is used in a behavior. self._topic = { "img": "bounding_box_2d", "sw": "/spacoty/get_dataset", "wrd": "/spacoty/place_name" } self._output_topic = output_topic self._depth_topic = depth_topic self._rgb_topic = rgb_topic self._camera_info_topic = camera_info_topic self._save_folder = "/root/HSR/catkin_ws/src/em_spco_tidy_up/training_data/" self._save_image = save_image self.camera_info = None self.rgb_image = None self.depth_image = None self.z_offset = z_offset # get object labels # self._yolo_yaml_path = "/root/HSR/catkin_ws/src/hsr_launch/config/darknet_ros/"+yolo_yaml # obj_yaml = rosparam.load_file(self._yolo_yaml_path) # self.obj_class = obj_yaml[0][0]["yolo_model"]["detection_classes"]["names"] self._yolo_yaml_path = "/root/HSR/catkin_ws/src/cv_detect_object/scripts/" + yolo_yaml obj_yaml = rosparam.load_file(self._yolo_yaml_path) self.obj_class = obj_yaml[0][0].values() # pass required clients as dict (topic: type) self._client = ProxyActionClient( {self._topic["img"]: hsr_common.msg.BoundingBox2DAction}) self.bridge = cv_bridge.CvBridge() # Subscriber self.sub_sw = ProxySubscriberCached({self._topic["sw"]: Bool}) self.sub_word = ProxySubscriberCached({self._topic["wrd"]: String}) # define the dimension self._data_dim = {"pose": 3, "object": len(self.obj_class), "word": 0} # It may happen that the action client fails to send the action goal. self._error = False self._get_image_error = False
def __init__(self, name, configFilePath): params = rosparam.load_file(configFilePath) for param, ns in params: try: rosparam.upload_params(ns, param) except ROSParamException, e: print >> sys.stderr, "ERROR: " + str(e) pass # ignore empty params
def set_goal_from_file(self, file_path): """Set the goal from a YAML file containing the action combination.""" from rosparam import load_file if not os.path.isfile(file_path): rospy.logerr('File with path "' + file_path + '" does not exists.') self.set_state(ActionState.ERROR) return self.set_goal_from_yaml(load_file(file_path))
def load_markers(): publisher = rospy.Publisher('~marker', MarkerArray, queue_size=1, latch=True) markerArray = MarkerArray(); file_path = rospy.get_param('~world_file') if not file_path: rospy.loginfo('World description file path is empty.') return if not isfile(file_path): rospy.logwarn('World description file with path "' + file_path + '" does not exists.') return parameters = load_file(file_path) if not parameters: return for model in parameters[0][0]['models']: marker = Marker(); if 'box' in model: marker.type = Marker.CUBE; model = model['box'] marker.color.a = 1.0; marker.color.r = 0.5; marker.color.g = 0.5; marker.color.b = 0.5; elif 'cylinder' in model: marker.type = Marker.CYLINDER; model = model['cylinder'] marker.color.a = 1.0; marker.color.r = 0.5; marker.color.g = 0.5; marker.color.b = 0.5; elif 'mesh' in model: marker.type = Marker.MESH_RESOURCE; model = model['mesh'] marker.mesh_resource = model['mesh_resource'] marker.mesh_use_embedded_materials = True else: continue marker.ns = model['name'] marker.header.frame_id = model['frame_id'] # marker.id = i # TODO Required? marker.action = Marker.ADD marker.pose.position.x = model['position']['x'] marker.pose.position.y = model['position']['y'] marker.pose.position.z = model['position']['z'] marker.pose.orientation.x = model['orientation']['x'] marker.pose.orientation.y = model['orientation']['y'] marker.pose.orientation.z = model['orientation']['z'] marker.pose.orientation.w = model['orientation']['w'] marker.scale.x = model['scale']['x'] marker.scale.y = model['scale']['y'] marker.scale.z = model['scale']['z'] markerArray.markers.append(marker) rospy.loginfo("World `%s` is published as marker.", marker.ns) publisher.publish(markerArray) rospy.spin()
def rosmodel2owl(configFilePath): # the file path should be given as argument, alternatively a ns can be added # params = rosparam.load_file(rospack.get_path('mros1_reasoner')+'/config/nav_config.yaml', '') # loads individuals to specify a single function and QalityAttributeTypes from the domain_ontology_file params = rosparam.load_file(configFilePath) for param, ns in params: try: rosparam.upload_params(ns, param) except: pass # ignore empty params ontology_pkg = rospy.get_param('ontology_pkg') ontology_path = rospy.get_param('ontology_path') ontology_file = os.path.join( rospack.get_path(ontology_pkg) + '/' + ontology_path) domain_ontology_pkg = rospy.get_param('domain_ontology_pkg') domain_ontology_path = rospy.get_param('domain_ontology_path') domain_ontology_file = os.path.join( rospack.get_path(domain_ontology_pkg) + '/' + domain_ontology_path) function = rospy.get_param('function') configs = rospy.get_param('configs') result_file = rospy.get_param('result_file') tomasys = get_ontology(ontology_file).load() onto = get_ontology(domain_ontology_file).load() rospy.loginfo('Loaded domain ontology: ' + domain_ontology_file) function_ = onto.search_one(iri='*' + function + '*') if function_ == None: print('The domain ontology provided does not contain a Function ', function) sys.exit(0) for config in configs: config_name = config file_name = config_name + '.rossystem' file_path = os.path.join(rospack.get_path(config_name), file_name) parser = RosSystemModelParser(file_path) model = parser.parse() sys_name = model.system_name[0] # create a FunctionDesign fd = tomasys.FunctionDesign(sys_name, namespace=onto, solvesF=function_) # create a QualityAttribute expected value for the FunctionDesign with the type indicated by the name of the param in the RosSystem and the value of the param (e.g. 0.5) for qa_param in model.global_parameters: qa_string = qa_param.param_name[0].replace('qa_', '') value = qa_param.param_value[0] qa = tomasys.QAvalue('{0}_{1}'.format(qa_param.param_name[0], sys_name), namespace=onto, isQAtype=onto.search_one(iri='*' + qa_string), hasValue=value) fd.hasQAestimation.append(qa) # save the ontology to a file onto.save(file=result_file, format='rdfxml')
def __init__(self): rospy.loginfo("Sarting info terminal reconfigure tasks") paramlist = rosparam.load_file(rospy.get_param("~config_file"))[0][0] self.server_list = [] for k, v in paramlist.items(): self.server_list.append( Server(name=k, location=v["location"], parameters=v["parameters"]))
def load_joint_position_file(self, filename): '''Returns the dict with the positions previously saved in a yaml file''' try: data = rosparam.load_file(filename) except rosparam.RosParamException: # If not found, check again in the current package try: filename = rospkg.RosPack().get_path('iris_sami') + '/yaml/' + filename data = rosparam.load_file(filename) except Exception as e: self.error_msg = "Can't load positions from'" + filename + "'\n" + str(e) rospy.logerr(self.error_msg) return False self.joint_positions = data[0][0] self.positions_file = filename return data[0][0]
def load_action_from_file_and_transform(file_path, source_frame_id='', position=[0, 0, 0], orientation=[0, 0, 0, 1]): from rosparam import load_file import os if not os.path.isfile(file_path): rospy.logerr('File with path "' + file_path + '" does not exists.') return None return parse_action(load_file(file_path), source_frame_id, position, orientation)
def __load_params(self): try: rospy.loginfo('Loading parameters') res = rosparam.load_file(self.config_filepath, self.ns, True)[0] rospy.loginfo('Loading done') return True except: rospy.logwarn('Failed to read data from config') return False
def file_open(self): os.chdir("/home/user/catkin_ws/src/navigation_exam/params") paramlist = rosparam.load_file("task2.yaml", default_namespace=None) for params, ns in paramlist: #ns,param for key, value in params.items(): rosparam.upload_params(ns, params) return self.point(key, value)
def __init__(self): ########### load yaml file ########### try: self.data = rosparam.load_file("location.yaml", default_namespace="artag") print "load yaml file success" #update to server for params, ns in self.data: rosparam.upload_params(ns, params) time.sleep(1) except: print "load yaml file faile" sys.exit(0) ########### load param ########### try: self.local1_con = int( rospy.get_param('~/artag/location1/Confidence')) self.local1_x = float(rospy.get_param('~/artag/location1/x')) self.local1_y = float(rospy.get_param('~/artag/location1/y')) self.local2_con = int( rospy.get_param('~/artag/location2/Confidence')) self.local2_x = float(rospy.get_param('~/artag/location2/x')) self.local2_y = float(rospy.get_param('~/artag/location2/y')) self.local3_con = int( rospy.get_param('~/artag/location3/Confidence')) self.local3_x = float(rospy.get_param('~/artag/location3/x')) self.local3_y = float(rospy.get_param('~/artag/location3/y')) self.local4_con = int( rospy.get_param('~/artag/location4/Confidence')) self.local4_x = float(rospy.get_param('~/artag/location4/x')) self.local4_y = float(rospy.get_param('~/artag/location4/y')) print "load param success" time.sleep(1) print "**********************************************" print "value:" print("local1 { con %d , x %.4f , y %.4f }" % (self.local1_con, self.local1_x, self.local1_y)) print("local2 { con %d , x %.4f , y %.4f }" % (self.local2_con, self.local2_x, self.local2_y)) print("local3 { con %d , x %.4f , y %.4f }" % (self.local3_con, self.local3_x, self.local3_y)) print("local4 { con %d , x %.4f , y %.4f }" % (self.local4_con, self.local4_x, self.local4_y)) print "**********************************************" time.sleep(1) except: print "load param faile" sys.exit(0) self.listener = tf.TransformListener() self.listener2 = tf.TransformListener() self.listener3 = tf.TransformListener() self.listener4 = tf.TransformListener()
def _get_map_params(self): paramlist=rosparam.load_file(self.map_path + ".yaml",default_namespace="map_params") for params, ns in paramlist: rosparam.upload_params(ns,params) origin = rospy.get_param('/map_params/origin') resolution = rospy.get_param('/map_params/resolution') return (origin, resolution)
def main(): rospy.init_node("optitrak_odom_remap") from optparse import OptionParser p = OptionParser() p.add_option('-f', '--file', dest="filename", default="", help="YAML file to save parameters in.") p.add_option('-l', '--load', dest="is_load", action="store_true", default=False, help="Load parameters from file.") p.add_option( '-t', '--train', dest="is_train", action="store_true", default=False, help= "Train by moving the head to different poses and capturing the relative positions." ) p.add_option( '-p', '--publish', dest="is_pub", action="store_true", default=False, help="Publish the transformation from optitrak to base_footprint.") (opts, args) = p.parse_args() if opts.filename == "": print "Bad command line parameters" p.print_help() return if opts.is_load: print "hi" params = rosparam.load_file(opts.filename, "optitrak_calibration")[0][0] rosparam.upload_params("optitrak_calibration", params) rospy.sleep(0.1) if opts.is_pub: publish_transform() return if opts.is_train: opti_mat, robot_mat = capture_data() opti_mat = np.mat(opti_mat).T robot_mat = np.mat(robot_mat).T pos, rot = umeyama_method(opti_mat, robot_mat) print np.linalg.norm(robot_mat - (rot * opti_mat + pos)) save_params(pos, rot, opts.filename) return
def main(): rospy.init_node("netft_gravity_zeroing") from optparse import OptionParser p = OptionParser() p.add_option('-f', '--file', dest="filename", default="", help="YAML file to save parameters in.") p.add_option('-l', '--load', dest="is_load", action="store_true", default=False, help="Load parameters from file.") p.add_option('-r', '--run', dest="is_run", action="store_true", default=False, help="Publish a zeroed wrench.") p.add_option('-t', '--train', dest="is_train", action="store_true", default=False, help="Train by moving the gripper to different poses and measuring the wrenches.") p.add_option('-p', '--pr2', dest="is_pr2", action="store_true", default=False, help="Will run automated data collection if this is the PR2.") p.add_option('-z', '--start_zero', dest="start_zero", action="store_true", default=False, help="Use the first value in to zero the sensor.") p.add_option('-n', '--ntrials', dest="n_trials", default="6,6,4", help="Number of trials for each of the last 3 joint angles to move through. (default: 6,6,4)") (opts, args) = p.parse_args() try: n_4, n_5, n_6 = [int(n_str) for n_str in opts.n_trials.split(',')] except: print "Bad --ntrials parameter (format: --ntrials 6,6,4)" p.print_help() return if opts.is_load: params = rosparam.load_file(opts.filename, rospy.get_name())[0][0] rosparam.upload_params(rospy.get_name(), params) if opts.is_run: rospy.sleep(0.1) nft_z = NetFTZeroer(start_zero=opts.start_zero, is_pr2=opts.is_pr2) rospy.spin() return if opts.is_train: if opts.is_pr2: data = collect_data_pr2(n_4, n_5, n_6) param_vector = process_data(data, True) save_params(param_vector, opts.filename) return else: data = collect_data_tool() param_vector = process_data(data, False) save_params(param_vector, opts.filename) return
def __init__(self, name): rospy.loginfo("Starting node: %s" % name) self.current_edge = 'none' self.current_node = 'none' self.alive = False self.white_list_edges = [] white_list_file = rospy.get_param("~white_list_file", '') if white_list_file == '': rospy.logfatal("No white list for logging specified") raise rospy.ROSException("No white list for logging specified") return white_list = rosparam.load_file(white_list_file)[0][0] rospy.loginfo("Getting topological map name...") self.map_name = None self.map_sub = rospy.Subscriber( rospy.get_param("~topo_map_topic", "/topological_map"), TopologicalMap, self.get_map_name) while not self.map_name and not rospy.is_shutdown(): rospy.logwarn("Topologial map is not being published. Waiting...") rospy.sleep(1) rospy.loginfo(" ... got topological map: %s" % self.map_name) rospy.loginfo("Getting allowed nodes from file...") self.white_list_nodes = white_list["nodes"] rospy.loginfo(" ... got nodes: %s" % self.white_list_nodes) if not [x for x in self.white_list_nodes if "ALL" in x]: while not rospy.is_shutdown(): try: rospy.loginfo("Creating edge look-up service...") s = rospy.ServiceProxy( '/topological_map_manager/get_edges_between_nodes', GetEdgesBetweenNodes) rospy.loginfo("... waiting for edge look-up service") s.wait_for_service() rospy.loginfo("... found edge look-up service.") for idx, nodea in enumerate(self.white_list_nodes): for nodeb in self.white_list_nodes[idx + 1:]: rospy.logdebug("%s <-> %s" % (nodea, nodeb)) req = GetEdgesBetweenNodesRequest() req.nodea = nodea req.nodeb = nodeb resp = s(req) if resp.ids_a_to_b and resp.ids_b_to_a: self.white_list_edges.append( str(resp.ids_a_to_b[0]) + '--' + self.map_name) self.white_list_edges.append( str(resp.ids_b_to_a[0]) + '--' + self.map_name) break except rospy.ServiceException, e: rospy.logwarn("Service call failed: %s. Retrying." % e) rospy.sleep(1)
def read_map(): """ Extract room numbers from the map. @return numbered_locations: dict {int: string} """ numbered_locations = rosparam.load_file( str(Path.home()) + "/catkin_ws/src/competition2/yaml/numbered_locations.yaml")[0][0] return numbered_locations
def LoadYamlFileParamsTest(rospackage_name, rel_path_from_package_to_file, yaml_file_name): rospack = rospkg.RosPack() pkg_path = rospack.get_path(rospackage_name) config_dir = os.path.join(pkg_path, rel_path_from_package_to_file) path_config_file = os.path.join(config_dir, yaml_file_name) paramlist=rosparam.load_file(path_config_file) for params, ns in paramlist: rosparam.upload_params(ns,params)
def __init__(self, name): rospy.loginfo("Starting node: %s" % name) self.current_edge = 'none' self.current_node = 'none' self.alive = False self.white_list_edges = [] white_list_file = rospy.get_param("~white_list_file", '') if white_list_file == '': rospy.logfatal("No white list for logging specified") raise rospy.ROSException("No white list for logging specified") return white_list = rosparam.load_file(white_list_file)[0][0] rospy.loginfo("Getting topological map name...") self.map_name = None self.map_sub = rospy.Subscriber( rospy.get_param("~topo_map_topic", "/topological_map"), TopologicalMap, self.get_map_name ) while not self.map_name and not rospy.is_shutdown(): rospy.logwarn("Topologial map is not being published. Waiting...") rospy.sleep(1) rospy.loginfo(" ... got topological map: %s" % self.map_name) rospy.loginfo("Getting allowed nodes from file...") self.white_list_nodes = white_list["nodes"] rospy.loginfo(" ... got nodes: %s" % self.white_list_nodes) if not [x for x in self.white_list_nodes if "ALL" in x]: while not rospy.is_shutdown(): try: rospy.loginfo("Creating edge look-up service...") s = rospy.ServiceProxy( '/topological_map_manager/get_edges_between_nodes', GetEdgesBetweenNodes ) rospy.loginfo("... waiting for edge look-up service") s.wait_for_service() rospy.loginfo("... found edge look-up service.") for idx, nodea in enumerate(self.white_list_nodes): for nodeb in self.white_list_nodes[idx+1:]: rospy.logdebug("%s <-> %s" % (nodea, nodeb)) req = GetEdgesBetweenNodesRequest() req.nodea = nodea req.nodeb = nodeb resp = s(req) if resp.ids_a_to_b and resp.ids_b_to_a: self.white_list_edges.append(str(resp.ids_a_to_b[0])+'--'+self.map_name) self.white_list_edges.append(str(resp.ids_b_to_a[0])+'--'+self.map_name) break except rospy.ServiceException, e: rospy.logwarn("Service call failed: %s. Retrying." % e) rospy.sleep(1)
def __init__(self): try: self.no_frames = rospy.get_param('/camera_calibration/frames'); except: self.no_frames = 1000 print self.no_frames self.pattern_size = (6, 8) self.pattern_points = np.zeros((np.prod(self.pattern_size), 3), np.float32) self.pattern_points[:, :2] = np.indices(self.pattern_size).T.reshape(-1, 2) self.pattern_points[:, 0] = -self.pattern_points[:, 0] self.pattern_points *= 0.05 self.count = [1, 1] self.left_midXYZ = [0, 0, 0] self.left_midQ = [0, 0, 0, 0] self.right_midXYZ = [0, 0, 0] self.right_midQ = [0, 0, 0, 0] self.final_left_xyz = [0, 0, 0] self.final_left_q = [0, 0, 0, 1] self.final_right_xyz = [0, 0, 0] self.final_right_q = [0, 0, 0, 1] self.still_calibrating = True self.bridge = CvBridge() self.left_image = rospy.Subscriber('/left/rgb/image_color', Image, self.left_callback) self.right_image = rospy.Subscriber('/right/rgb/image_color', Image, self.right_callback) self.br = tf.TransformBroadcaster() self.listener = tf.TransformListener() self.pt = {'previous_transform': {'left': {'orientation': [0.0, 0.0, 0.0, 0.0], 'position': [0.0, 0.0, 0.0]}, 'right': {'orientation': [0.0, 0.0, 0.0, 0.0], 'position': [0.0, 0.0, 0.0]}}} self.rospack = rospkg.RosPack() try: self.pt = rosparam.load_file(self.rospack.get_path('camera_setup') + '/config/previous_transform.yaml')[0][0] except: self.still_calibrating = True if rospy.get_param('~quick'): self.final_left_xyz = self.pt['previous_transform']['left']['position'] self.final_left_q = self.pt['previous_transform']['left']['orientation'] self.final_right_xyz = self.pt['previous_transform']['right']['position'] self.final_right_q = self.pt['previous_transform']['right']['orientation'] self.still_calibrating = False self.publish_feedback()
def main(): rospy.init_node("optitrak_odom_remap") from optparse import OptionParser p = OptionParser() p.add_option("-f", "--file", dest="filename", default="", help="YAML file to save parameters in.") p.add_option("-l", "--load", dest="is_load", action="store_true", default=False, help="Load parameters from file.") p.add_option( "-t", "--train", dest="is_train", action="store_true", default=False, help="Train by moving the head to different poses and capturing the relative positions.", ) p.add_option( "-p", "--publish", dest="is_pub", action="store_true", default=False, help="Publish the transformation from optitrak to base_footprint.", ) (opts, args) = p.parse_args() if opts.filename == "": print "Bad command line parameters" p.print_help() return if opts.is_load: print "hi" params = rosparam.load_file(opts.filename, "optitrak_calibration")[0][0] rosparam.upload_params("optitrak_calibration", params) rospy.sleep(0.1) if opts.is_pub: publish_transform() return if opts.is_train: opti_mat, robot_mat = capture_data() opti_mat = np.mat(opti_mat).T robot_mat = np.mat(robot_mat).T pos, rot = umeyama_method(opti_mat, robot_mat) print np.linalg.norm(robot_mat - (rot * opti_mat + pos)) save_params(pos, rot, opts.filename) return
def write_and_start(filename, buffer_string): f = open(filename, "w+") f.write(buffer_string) f.close() paramlist = rosparam.load_file(filename) for params, ns in paramlist: rosparam.upload_params(ns, params) if ON: # send start command to smach! start = rospy.ServiceProxy("/start_SMACH", Empty) try: rospy.wait_for_service('/start_SMACH') start() except rospy.ServiceException, e: print "Service call failed: %s" % e
def __init__(self, *args): super(TestAddAnalyzer, self).__init__(*args) rospy.init_node('test_add_analyzer') self.namespace = rospy.get_name() paramlist = rosparam.load_file(rospy.myargv()[1]) # expect to receive these paths in the added analyzers self.expected = [paramlist[0][1] + analyzer['path'] for name, analyzer in paramlist[0][0]['analyzers'].iteritems()] self._mutex = threading.Lock() self.agg_msgs = {} # put parameters in the node namespace so they can be read by the aggregator for params, ns in paramlist: rosparam.upload_params(rospy.get_name() + '/' + ns, params) rospy.Subscriber('/diagnostics_agg', DiagnosticArray, self.agg_cb) self.pub = rospy.Publisher('/diagnostics', DiagnosticArray, queue_size=1)
def switch(self, old_controller, new_controller, param_file=None): if param_file is None: self.load_controller(new_controller) resp = self.switch_controller_srv([new_controller], [old_controller], 1) self.unload_controller(old_controller) return resp.ok else: params = rosparam.load_file(roslib.substitution_args.resolve_args(param_file)) rosparam.upload_params("", params[0][0]) self.switch_controller_srv([], [old_controller], 1) self.unload_controller(old_controller) if old_controller != new_controller: self.unload_controller(new_controller) self.load_controller(old_controller) if old_controller != new_controller: self.load_controller(new_controller) resp = self.switch_controller_srv([new_controller], [], 1) return resp.ok
def carefree_switch(self, arm, new_controller, param_file=None, possible_controllers=None): if '%s' in new_controller: new_controller = new_controller % arm if param_file is not None: params = rosparam.load_file(roslib.substitution_args.resolve_args(param_file)) rosparam.upload_params("", params[0][0]) if possible_controllers is None: possible_controllers = POSSIBLE_ARM_CONTROLLERS check_arm_controllers = [controller % arm for controller in possible_controllers] resp = self.list_controllers_srv() start_controllers, stop_controllers = [new_controller], [] for i, controller in enumerate(resp.controllers): if controller in check_arm_controllers and resp.state[i] == 'running': stop_controllers.append(controller) self.load_controller(new_controller) rospy.loginfo("[pr2_controller_switcher] Starting controller %s" % (start_controllers[0]) + " and stopping controllers: [" + ",".join(stop_controllers) + "]") resp = self.switch_controller_srv(start_controllers, stop_controllers, 1) return resp.ok
def update(self): self.collections = [] self.collections_to_merge = [] self.collections_to_ignore = [] rospack = rospkg.RosPack() packages = rospack.get_depends_on(self.name, implicit=False) for package in packages: manifest = rospack.get_manifest(package) file_path = manifest.get_export(self.name, 'collections') if not file_path: continue elif len(file_path) != 1: rospy.logwarn("Cannot load collections [%s]: invalid 'collections' attribute."%(pkg)) continue file_path = file_path[0] try: if not os.path.isfile(file_path): rospy.logwarn('Collections parameter file with path "' + file_path + '" does not exists.') continue parameters = load_file(file_path) for collections_parameters in parameters[0][0]['collections']: if 'collection' in collections_parameters: collection = Collection(collections_parameters['collection']) self.collections.append(collection) elif 'add_to_collection' in collections_parameters: collection = Collection(collections_parameters['add_to_collection']) self.collections_to_merge.append(collection) elif 'ignore_collection' in collections_parameters: collection = Collection(collections_parameters['ignore_collection']) self.collections_to_ignore.append(collection) except Exception: rospy.logwarn("Unable to load collections [%s] from package [%s]."%(file_path, package)) self._merge_collections() self._ignore_collections() self.collections = sorted(self.collections, key = lambda x: (x.id)) return True
def carefree_switch(self, arm, new_controller, param_file=None, reset=True): if '%s' in new_controller: new_ctrl = new_controller % arm else: new_ctrl = new_controller if param_file is not None: params = rosparam.load_file(roslaunch.substitution_args.resolve_args(param_file)) if new_ctrl not in params[0][0]: rospy.logwarn("[pr2_controller_switcher] Controller not in parameter file.") return else: rosparam.upload_params("", {new_ctrl : params[0][0][new_ctrl]}) possible_controllers = rosparam.get_param(LOADED_CTRLS_PARAMS[arm]) if new_ctrl not in possible_controllers: possible_controllers.append(new_ctrl) rosparam.set_param_raw(LOADED_CTRLS_PARAMS[arm], possible_controllers) check_arm_controllers = [] for controller in possible_controllers: if '%s' in controller: controller = controller % arm if controller[0] == arm: check_arm_controllers.append(controller) resp = self.list_controllers_srv() start_controllers, stop_controllers = [new_ctrl], [] for i, controller in enumerate(resp.controllers): if controller in check_arm_controllers and resp.state[i] == 'running': stop_controllers.append(controller) if controller == new_ctrl: if resp.state[i] == 'running': if not reset: rospy.loginfo("[pr2_controller_switcher] Specified controller is already running.") return True self.switch_controller_srv([], [new_ctrl], 1) self.unload_controller(new_ctrl) self.load_controller(new_ctrl) rospy.loginfo("[pr2_controller_switcher] Starting controller %s" % (start_controllers[0]) + " and stopping controllers: [" + ",".join(stop_controllers) + "]") resp = self.switch_controller_srv(start_controllers, stop_controllers, 1) return resp.ok
def load_action_from_file(file_path, placeholders=None): import os from rosparam import load_file if not os.path.isfile(file_path): rospy.logerr('File with path "' + file_path + '" does not exists.') return None parameters = load_file(file_path) # Replace placeholders. if placeholders is not None: replace_placeholders(parameters[0][0], placeholders) # Adapt coordinates. is_adapt = False source_frame_id = '' target_frame_id = '' position = [0, 0, 0] orientation = [0, 0, 0, 1] if 'adapt_coordinates' in parameters[0][0]: is_adapt = True adapt_parameters = parameters[0][0]['adapt_coordinates'][0]['transform'] source_frame_id = adapt_parameters['source_frame'] target_frame_id = adapt_parameters['target_frame'] if 'transform_in_source_frame' in adapt_parameters: if 'position' in adapt_parameters['transform_in_source_frame']: position = adapt_parameters['transform_in_source_frame']['position'] if 'orientation' in adapt_parameters['transform_in_source_frame']: orientation = adapt_parameters['transform_in_source_frame']['orientation'] if len(orientation) == 3: orientation = quaternion_from_euler(orientation[0], orientation[1], orientation[2]) if is_adapt: try: (position, orientation) = transform_coordinates(source_frame_id, target_frame_id, position, orientation) except TypeError: return None return parse_action(parameters, source_frame_id, target_frame_id, position, orientation)
def enable_controller_cb(self, req): if req.enable: face_adls_modes = rospy.get_param('/face_adls_modes', None) params = face_adls_modes[req.mode] self.face_side = rospy.get_param('/face_side', 'r') # TODO Make more general self.trim_retreat = req.mode == "shaving" self.flip_gripper = self.face_side == 'r' and req.mode == "shaving" bounds = params['%s_bounds' % self.face_side] self.ellipsoid.set_bounds(bounds['lat'], bounds['lon'], bounds['height'])#TODO: Change Back #self.ellipsoid.set_bounds((-np.pi,np.pi), (-np.pi,np.pi), (0,100)) success, e_params = self.register_ellipse(req.mode, self.face_side) if not success: self.publish_feedback(Messages.NO_PARAMS_LOADED) return EnableFaceControllerResponse(False) reg_pose = PoseConv.to_pose_stamped_msg(e_params.e_frame) try: now = rospy.Time.now() reg_pose.header.stamp = now self.tfl.waitForTransform(reg_pose.header.frame_id, '/base_link', now, rospy.Duration(8.0)) ellipse_frame_base = self.tfl.transformPose('/base_link', reg_pose) except (LookupException, ConnectivityException, ExtrapolationException, Exception) as e: rospy.logwarn("[face_adls_manager] TF Failure converting ellipse to base frame: %s" %e) self.ellipsoid.load_ell_params(ellipse_frame_base, e_params.E, e_params.is_oblate, e_params.height) global_poses_dir = rospy.get_param("~global_poses_dir", "") global_poses_file = params["%s_ell_poses_file" % self.face_side] global_poses_resolved = roslaunch.substitution_args.resolve_args( global_poses_dir + "/" + global_poses_file) self.global_poses = rosparam.load_file(global_poses_resolved)[0][0] self.global_move_poses_pub.publish(sorted(self.global_poses.keys())) #Check rotating gripper based on side of body if self.flip_gripper: self.gripper_rot = trans.quaternion_from_euler(np.pi, 0, 0) print "Rotating gripper by PI around x-axis" else: self.gripper_rot = trans.quaternion_from_euler(0, 0, 0) print "NOT Rotating gripper around x-axis" # check if arm is near head # cart_pos, cart_quat = self.current_ee_pose(self.ee_frame) # ell_pos, ell_quat = self.ellipsoid.pose_to_ellipsoidal((cart_pos, cart_quat)) # equals = ell_pos == self.ellipsoid.enforce_bounds(ell_pos) # print ell_pos, equals # if self.ellipsoid._lon_bounds[0] >= 0 and ell_pos[1] >= 0: # arm_in_bounds = np.all(equals) # else: # ell_lon_1 = np.mod(ell_pos[1], 2 * np.pi) # min_lon = np.mod(self.ellipsoid._lon_bounds[0], 2 * np.pi) # arm_in_bounds = (equals[0] and # equals[2] and # (ell_lon_1 >= min_lon or ell_lon_1 <= self.ellipsoid._lon_bounds[1])) arm_in_bounds = True self.setup_force_monitor() success = True if not arm_in_bounds: self.publish_feedback(Messages.ARM_AWAY_FROM_HEAD) success = False #Switch back to l_arm_controller if necessary if self.controller_switcher.carefree_switch('l', '%s_arm_controller', reset=False): print "Controller switch to l_arm_controller succeeded" self.publish_feedback(Messages.ENABLE_CONTROLLER) else: print "Controller switch to l_arm_controller failed" success = False self.controller_enabled_pub.publish(Bool(success)) req = EnableHapticMPCRequest() req.new_state = 'enabled' resp = self.enable_mpc_srv.call(req) else: self.stop_moving() self.controller_enabled_pub.publish(Bool(False)) rospy.loginfo(Messages.DISABLE_CONTROLLER) req = EnableHapticMPCRequest() req.new_state = 'disabled' self.enable_mpc_srv.call(req) success = False return EnableFaceControllerResponse(success)
from util_states.image_to_cv import image_converter import os import shutil import subprocess from geometry_msgs.msg._PoseWithCovarianceStamped import PoseWithCovarianceStamped # Loading the parameters depending on which robot we are using. # Reduce the size of image file robot = os.environ.get('PAL_ROBOT') if robot == 'rh2c' or robot == 'rh2m': IMAGE_PATH = roslib.packages.get_pkg_dir('reemh2_maps') + '/config/' RH2_PATH = roslib.packages.get_pkg_dir('reemh2_maps') paramlist = rosparam.load_file(RH2_PATH+"/config/map.yaml", default_namespace="emergency_situation") for params, ns in paramlist: rosparam.upload_params(ns, params) elif robot == 'reemh3c' or robot == 'reemh3m': IMAGE_PATH = roslib.packages.get_pkg_dir('reem_maps') + '/config/' REEMH3_PATH = roslib.packages.get_pkg_dir('reem_maps') paramlist = rosparam.load_file(REEMH3_PATH+'/config/map.yaml', default_namespace="emergency_situation") for params, ns in paramlist: rosparam.upload_params(ns, params) else: #IMAGE_PATH = roslib.packages.get_pkg_dir('robocup_iworlds') + '/navigation/' IMAGE_PATH = roslib.packages.get_pkg_dir('emergency_situation') + '/config/' #ROBOCUP_PATH = roslib.packages.get_pkg_dir('robocup_worlds') #paramlist = rosparam.load_file(ROBOCUP_PATH+"/navigation/subMap1.yaml", default_namespace="emergency_situation")
def reconfigure(self): try: yaml = rospy.get_param('/tracking/scenario') except KeyError: print("could not find parameter: tracking/scenario on server") print("Waiting for reset message (/tracking/reset_all) ...") self.mode = "stop" return try: param,ns = rosparam.load_file(yaml)[0] except rosparam.RosParamException: print("Failed to load %s" %yaml) print("Waiting for reset message (/tracking/reset_all) ...") self.mode = "stop" return print("Loaded %s" %yaml) try: self.entities = [ Entity(**p) for p in param['entities'] ] except NameError as e: print("Couldn't instantiate entities.") print("Something seems wrong with the entities section of your yaml file:") print("\t%s"%e) print("Waiting for reset message (/tracking/reset_all) ...") self.mode = "stop" return except TypeError as e: print("Couldn't instantiate entities.") print("Something seems wrong with the entities section of your yaml file:") print("\t%s"%e) print(Entity.__doc__) print("Waiting for reset message (/tracking/reset_all) ...") self.mode = "stop" return except: print(sys.exc_info()) raise print("Entity creation successful. (%s entities loaded)" % len(self.entities)) try: self.world = World(**param['world']) except NameError as e: print("Couldn't instantiate simulation.") print("Something seems wrong with the world section of your yaml file.") print("\t%s"%e) print("Waiting for reset message (/tracking/reset_all) ...") self.mode = "stop" return except TypeError as e: print("Couldn't instantiate simulation.") print("Something seems wrong with the world section of your yaml file.") print("\t%s"%e) print(World.__doc__) print("Waiting for reset message (/tracking/reset_all) ...") self.mode = "stop" return except: print(sys.exc_info()) raise self.mode = "play" print("Simulator creation successful.") print("Default output points [mlr_msgs::Point2dArray], topic is: tracking/lk2d/points") print("Default output images [sensor_msgs::Image], topic is: camera/rgb/image_color") self.world.set_entities(self.entities) print("Running...")
def format_data_three_categories(self, subject, result): # print subject # print result print 'Now editing files to insert position data and storing them in labeled folders.' paramlist = rosparam.load_file(''.join([self.data_path, '/', subject, '/params.yaml'])) for params, ns in paramlist: rosparam.upload_params(ns, params) arm_length = rosparam.get_param('crook_to_fist') fist_length = arm_length - rosparam.get_param('crook_to_wrist') print 'Fist length: ', fist_length, ' cm' position_of_initial_contact = (44.0 - arm_length)/100. position_profile = None for ind_i in xrange(len(result)): for ind_j in xrange(len(result[0])): if ind_j == 2: continue else: if result[ind_i][ind_j] is not None: if ind_i < len(result)/2: load_num = ind_i vel = 0.1 else: load_num = ind_i - len(result)/2 vel = 0.15 if vel == 0.1: # print ''.join([self.data_path, '/position_profiles/position_combined_0_1mps.pkl']) position_profile = load_pickle(''.join([self.data_path, '/position_profiles/position_combined_0_1mps.pkl'])) # print 'Position profile loaded!' elif vel == 0.15: position_profile = load_pickle(''.join([self.data_path, '/position_profiles/position_combined_0_15mps.pkl'])) # print ''.join([self.data_path, '/position_profiles/position_combined_0_15mps.pkl']) # print 'Position profile loaded!' else: print 'There is no saved position profile for this velocity! Something has gone wrong!' return None ft_threshold_was_exceeded = False # current_data = np.array([map(float,line.strip().split()) for line in open(''.join([self.data_path, '/', subject, '/', input_classes[class_num], '/ft_sleeve_', str(load_num), '.log']))]) # while os.path.isfile(''.join([self.pkg_path, '/data/', subject, '/',str(vel),'mps/', input_classes[class_num], '/ft_sleeve_', str(i), '.pkl'])): ft_threshold_was_exceeded = False # print ''.join([self.pkg_path, '/data/', subject, '/', input_classes[class_num], '/ft_sleeve_', str(i), '.pkl']) current_data = np.array([map(float,line.strip().split()) for line in open(''.join([self.data_path, '/', subject, '/', str(vel), 'mps/height', str(ind_j), '/ft_sleeve_', str(load_num), '.log']))]) del_index = [] for k in xrange(len(current_data)): if current_data[k][0] < 5.0: del_index.append(k) current_data = np.delete(current_data, del_index, 0) position_of_stop = 0. del_index = [] time_of_initial_contact = None # current_data = load_pickle(''.join([self.pkg_path, '/data/', subject, '/', input_classes[class_num], '/ft_sleeve_', str(i), '.pkl'])) # if np.max(current_data[:, 2]) >= 10. or np.max(current_data[:, 3]) >= 10. \ # or np.max(current_data[:, 4]) >= 10.: # ft_threshold_was_exceeded = True position_of_stop = 0. del_index = [] for num, j in enumerate(current_data): j[2] = -j[2] j[3] = -j[3] j[4] = -j[4] j[0] = j[0] - 2.0 # if j[0] < 0.5: # j[1] = 0 if np.max(np.abs(j[2:5])) > 10. and not ft_threshold_was_exceeded: time_of_stop = j[0] ft_threshold_was_exceeded = True for k in xrange(len(position_profile)-1): if position_profile[k, 0] <= j[0] < position_profile[k+1, 0]: position_of_stop = position_profile[k, 1] + \ (position_profile[k+1, 1] - position_profile[k, 1]) * \ (j[0]-position_profile[k, 0])/(position_profile[k+1, 0] - position_profile[k, 0]) j[1] = new_position elif ft_threshold_was_exceeded: del_index.append(num) # j[1] = position_of_stop else: for k in xrange(len(position_profile)-1): if position_profile[k, 0] <= j[0] < position_profile[k+1, 0]: new_position = position_profile[k, 1] + \ (position_profile[k+1, 1] - position_profile[k, 1]) * \ (j[0]-position_profile[k, 0])/(position_profile[k+1, 0] - position_profile[k, 0]) j[1] = new_position if abs(new_position - position_of_stop) < 0.0005 and new_position > 0.8: del_index.append(num) elif new_position < position_of_initial_contact: del_index.append(num) else: if time_of_initial_contact is None: time_of_initial_contact = j[0] position_of_stop = new_position # if result[ind_i][ind_j] == 'good' current_data = np.delete(current_data, del_index, 0) output_data = [] for item in current_data: output_data.append([item[0]-time_of_initial_contact, item[1]-position_of_initial_contact, item[2], item[3], item[4]]) output_data = np.array(output_data) save_number = 0 if result[ind_i][ind_j] == 'caught_fist' or result[ind_i][ind_j] == 'caught_other': save_file = ''.join([self.data_path, '/', subject, '/formatted_three/', str(vel), 'mps/', 'caught', '/force_profile_', str(save_number), '.pkl']) else: save_file = ''.join([self.data_path, '/', subject, '/formatted_three/', str(vel), 'mps/', result[ind_i][ind_j], '/force_profile_', str(save_number), '.pkl']) while os.path.isfile(save_file): save_number += 1 if result[ind_i][ind_j] == 'caught_fist' or result[ind_i][ind_j] == 'caught_other': save_file = ''.join([self.data_path, '/', subject, '/formatted_three/', str(vel), 'mps/', 'caught', '/force_profile_', str(save_number), '.pkl']) else: save_file = ''.join([self.data_path, '/', subject, '/formatted_three/', str(vel), 'mps/', result[ind_i][ind_j], '/force_profile_', str(save_number), '.pkl']) print 'Saving with file name', save_file save_pickle(output_data, save_file) print 'Done editing files!'
def _parse(self, file_name): param_list = rosparam.load_file(file_name, default_namespace='/') super(Yaml, self).__init__(param_list[0][0])
def __init__(self): rospy.init_node("config_manager") rospy.on_shutdown(self._on_node_shutdown) if not ros_datacentre.util.wait_for_mongo(): sys.exit(1) self._mongo_client = pymongo.MongoClient(rospy.get_param("datacentre_host","localhost"), int(rospy.get_param("datacentre_port"))) # Load the default settings from the defaults/ folder path = os.path.join(roslib.packages.get_pkg_dir('ros_datacentre'),"defaults") files = os.listdir(path) defaults=[] # a list of 3-tuples, (param, val, originating_filename) def flatten(d, c="", f_name="" ): l=[] for k, v in d.iteritems(): if isinstance(v, collections.Mapping): l.extend(flatten(v,c+"/"+k, f_name)) else: l.append((c+"/"+k, v, f_name)) return l for f in files: if not f.endswith(".yaml"): continue params = rosparam.load_file(os.path.join(path,f)) rospy.loginfo("Found default parameter file %s" % f) for p, n in params: defaults.extend(flatten(p,c="",f_name=f)) # Copy the defaults into the DB if not there already defaults_collection = self._mongo_client.config.defaults for param,val,filename in defaults: existing = defaults_collection.find_one({"path":param}) if existing is None: rospy.loginfo("New default parameter for %s"%param) defaults_collection.insert({"path":param, "value":val, "from_file":filename}) elif existing["from_file"]!=filename: rospy.logerr("Two defaults parameter files have the same key:\n%s and %s, key %s"% (existing["from_file"],filename,param)) # Delete the entry so that it can be fixed... defaults_collection.remove(existing) rospy.signal_shutdown("Default parameter set error") elif existing["value"]!=val: rospy.loginfo("Updating stored default for %s"%param) defaults_collection.update(existing,{"$set":{"value":val}}) # Load the settings onto the ros parameter server defaults_collection = self._mongo_client.config.defaults local_collection = self._mongo_client.config.local for param in defaults_collection.find(): name=param["path"] val=param["value"] if local_collection.find_one({"path":name}) is None: rospy.set_param(name,val) for param in local_collection.find(): name=param["path"] val=param["value"] rospy.set_param(name,val) # Advertise ros services for parameter setting / getting self._getparam_srv = rospy.Service("/config_manager/get_param", GetParam, self._getparam_srv_cb) self._setparam_srv = rospy.Service("/config_manager/set_param", SetParam, self._setparam_srv_cb) self._saveparam_srv = rospy.Service("/config_manager/save_param", SetParam, self._saveparam_srv_cb) #self._list_params() # Start the main loop rospy.spin()
def autolabel_and_set_position(self, subject, result): print 'Now inserting position data and labeling results automatically according to where they stopped.' paramlist = rosparam.load_file(''.join([self.data_path, '/', subject, '/params.yaml'])) for params, ns in paramlist: rosparam.upload_params(ns, params) arm_length = rosparam.get_param('crook_to_fist') fist_length = arm_length - rosparam.get_param('crook_to_wrist') print 'Fist length: ', fist_length, ' cm' position_of_initial_contact = (44.0 - arm_length)/100. position_profile = load_pickle(''.join([self.data_path, '/position_profiles/position_combined_0_1mps.pkl'])) vel = 0.1 ft_threshold_was_exceeded = False reference_data = np.array([map(float,line.strip().split()) for line in open(''.join([self.data_path, '/', subject, '/', str(vel), 'mps/height3/ft_sleeve_0.log']))]) del_index = [] for k in xrange(len(reference_data)): if reference_data[k][0] < 5.0: del_index.append(k) reference_data = np.delete(reference_data, del_index, 0) del_index = [] for num, j in enumerate(reference_data): j[2] = -j[2] j[3] = -j[3] j[4] = -j[4] j[0] = j[0] - 2.0 # if j[0] < 0.5: # j[1] = 0 if np.max(np.abs(j[2:5])) > 10. and not ft_threshold_was_exceeded: time_of_stop = j[0] ft_threshold_was_exceeded = True for k in xrange(len(position_profile)-1): if position_profile[k, 0] <= j[0] < position_profile[k+1, 0]: success_position_of_stop = position_profile[k, 1] + \ (position_profile[k+1, 1] - position_profile[k, 1]) * \ (j[0]-position_profile[k, 0])/(position_profile[k+1, 0] - position_profile[k, 0]) elif ft_threshold_was_exceeded: del_index.append(num) # j[1] = position_of_stop else: for k in xrange(len(position_profile)-1): if position_profile[k, 0] <= j[0] < position_profile[k+1, 0]: new_position = position_profile[k, 1] + \ (position_profile[k+1, 1] - position_profile[k, 1]) * \ (j[0]-position_profile[k, 0])/(position_profile[k+1, 0] - position_profile[k, 0]) j[1] = new_position print 'The stop position when the sleeve reaches the elbow is: ', success_position_of_stop position_profile = None for ind_i in xrange(len(result)): for ind_j in xrange(len(result[0])): if result[ind_i][ind_j] is not None: if ind_i < len(result)/2: load_num = ind_i vel = 0.1 else: load_num = ind_i - len(result)/2 vel = 0.15 if vel == 0.1: # print ''.join([self.data_path, '/position_profiles/position_combined_0_1mps.pkl']) position_profile = load_pickle(''.join([self.data_path, '/position_profiles/position_combined_0_1mps.pkl'])) # print 'Position profile loaded!' elif vel == 0.15: position_profile = load_pickle(''.join([self.data_path, '/position_profiles/position_combined_0_15mps.pkl'])) # print ''.join([self.data_path, '/position_profiles/position_combined_0_15mps.pkl']) # print 'Position profile loaded!' else: print 'There is no saved position profile for this velocity! Something has gone wrong!' return None ft_threshold_was_exceeded = False current_data = np.array([map(float,line.strip().split()) for line in open(''.join([self.data_path, '/', subject, '/', str(vel), 'mps/height', str(ind_j), '/ft_sleeve_', str(load_num), '.log']))]) del_index = [] for k in xrange(len(current_data)): if current_data[k][0] < 5.0: del_index.append(k) current_data = np.delete(current_data, del_index, 0) position_of_stop = 0. del_index = [] time_of_initial_contact = None for num, j in enumerate(current_data): j[2] = -j[2] j[3] = -j[3] j[4] = -j[4] j[0] = j[0] - 2.0 # if j[0] < 0.5: # j[1] = 0 if np.max(np.abs(j[2:5])) > 10. and not ft_threshold_was_exceeded: time_of_stop = j[0] ft_threshold_was_exceeded = True for k in xrange(len(position_profile)-1): if position_profile[k, 0] <= j[0] < position_profile[k+1, 0]: position_of_stop = position_profile[k, 1] + \ (position_profile[k+1, 1] - position_profile[k, 1]) * \ (j[0]-position_profile[k, 0])/(position_profile[k+1, 0] - position_profile[k, 0]) j[1] = new_position elif ft_threshold_was_exceeded: del_index.append(num) # j[1] = position_of_stop else: for k in xrange(len(position_profile)-1): if position_profile[k, 0] <= j[0] < position_profile[k+1, 0]: new_position = position_profile[k, 1] + \ (position_profile[k+1, 1] - position_profile[k, 1]) * \ (j[0]-position_profile[k, 0])/(position_profile[k+1, 0] - position_profile[k, 0]) j[1] = new_position if abs(new_position - position_of_stop) < 0.0005 and new_position > 0.8: del_index.append(num) elif new_position < position_of_initial_contact: del_index.append(num) else: if time_of_initial_contact is None: time_of_initial_contact = j[0] position_of_stop = new_position # if num > 8 and 0.05 < new_position < 0.4 and position_of_initial_contact is None: # prev_f_x_average = np.mean(current_data[num-7:num,2]) # curr_f_x_average = np.mean(current_data[num-6:num+1,2]) # prev_f_z_average = np.mean(current_data[num-7:num,4]) # curr_f_z_average = np.mean(current_data[num-6:num+1,4]) # if curr_f_x_average < -0.08 and curr_f_x_average < prev_f_x_average and curr_f_z_average < prev_f_z_average and curr_f_z_average < -0.03: # position_of_initial_contact = copy.copy(new_position) # if position_of_initial_contact is None: # print 'Position of initial contact was not detected for ', subject, ' and trial ', ind_j, 'at height ', ind_i # position_of_initial_contact = 0. # # return # else: # print 'Position of initial contact is: ', position_of_initial_contact # current_data = np.delete(current_data, del_index, 0) # del_index = [] # for i in xrange(len(current_data)): # if current_data[i, 1] < position_of_initial_contact: # del_index.append(i) current_data = np.delete(current_data, del_index, 0) output_data = [] for item in current_data: output_data.append([item[0]-time_of_initial_contact, item[1]-position_of_initial_contact, item[2], item[3], item[4]]) output_data = np.array(output_data) print 'This trial failed at: ', position_of_stop this_label = None if not ft_threshold_was_exceeded: this_label = 'missed' elif abs(position_of_stop - success_position_of_stop) < 0.015 or position_of_stop >= success_position_of_stop: this_label = 'good' else: if abs(position_of_stop - success_position_of_stop) + fist_length/100. + 0.05 >= arm_length/100.: this_label = 'caught_fist' else: this_label = 'caught_other' save_number = 0 save_file = ''.join([self.data_path, '/', subject, '/auto_labeled/', str(vel), 'mps/', this_label, '/force_profile_', str(save_number), '.pkl']) while os.path.isfile(save_file): save_number += 1 save_file = ''.join([self.data_path, '/', subject, '/auto_labeled/', str(vel), 'mps/', this_label, '/force_profile_', str(save_number), '.pkl']) print 'Saving with file name', save_file save_pickle(output_data, save_file) print 'Done editing files!'
def setUp(self): sub8_thruster_mapper = rospack.get_path('sub8_thruster_mapper') self.thruster_layout = rosparam.load_file(sub8_thruster_mapper + '/config/thruster_layout.yaml')[0][0]
s = list(thruster_port.online_thruster_names) fprint(msg.reset.text('\n\tName: ').set_yellow.text(port).reset .text('\n\tMotor id\'s on port: ').set_cyan.bold(s).reset) except SubjuGatorException: pass return port_dict rospy.init_node('thruster_spinner') # Connect to thrusters sub8_thruster_mapper = rospack.get_path('sub8_thruster_mapper') thruster_layout = rosparam.load_file(sub8_thruster_mapper + '/config/thruster_layout.yaml')[0][0] thruster_ports = ports_from_layout(thruster_layout) if thruster_ports == {}: fprint('Unable to connect to any thruster ports. Quitting.') sys.exit() help_msg = \ ''' Welcome to David's ThrusterSpinner.\n Help: * press 'i' to toggle Individual mode on or off + Individual mode allows setting thrust values for individual thrusters: - press a motor_id to select a thruster - press up or down to change the thrust for that specific thruster - press '*' to clear reset all thrusts to 0 + Having Individual mode off allows all thrusters to be commanded with a single thrust value:
print "\nWelcome to David's M5 Thruster Shell\n" import rospy print 'Imported rospy' import sub8_thruster_comm.thruster_comm as tc print 'Imported sub8_thruster_comm.thruster_comm as tc' import sub8_thruster_comm.protocol as tp print 'Imported sub8_thruster_comm.protocol as tp' import rosparam print 'Imported rosparam' import numpy as np print 'Imported numpy as np' import os print 'Imported os' rospy.init_node('m5_debug_shell') rosparam.load_file(os.environ['CATKIN_DIR'] + '/src/SubjuGator/gnc/sub8_thruster_mapper/config/thruster_layout.yaml')[0][0] layout = rosparam.load_file(os.environ['CATKIN_DIR'] + '/src/SubjuGator/gnc/sub8_thruster_mapper/config/thruster_layout.yaml')[0][0] port_defs, thruster_defs = layout['thruster_ports'], layout['thrusters'] print "Port definitions and thruster definitions available as 'port_defs' and 'thruster_defs' respectively" tports = [] for i in range(4): t = None try: t = tc.ThrusterPort(port_defs[i], thruster_defs) except tc.VRCSRException as e: print e tports.append(t) t0, t1, t2, t3 = tports