Python rospkg.RosPack() Examples

def __init__(self):
        # Audio stream input setup
        FORMAT = pyaudio.paInt16
        CHANNELS = 1
        RATE = 16000
        self.CHUNK = 4096
        self.audio = pyaudio.PyAudio()
        self.stream = self.audio.open(format=FORMAT, channels=CHANNELS,
                                      rate=RATE, input=True,
                                      frames_per_buffer=self.CHUNK,
                                      stream_callback=self.get_data)
        self._buff = Queue.Queue()  # Buffer to hold audio data
        self.closed = False

        # ROS Text Publisher
        self.text_pub = rospy.Publisher('/google_client/text', String, queue_size=10)

        # Context clues in yaml file
        rospack = rospkg.RosPack()
        yamlFileDir = rospack.get_path('dialogflow_ros') + '/config/context.yaml'
        with open(yamlFileDir, 'r') as f:
            self.context = yaml.load(f) 

def spawn_model(model_name):
    """ Spawns a model in front of the RGBD camera.

        Args: None

        Returns: None
    """
    initial_pose = Pose()
    initial_pose.position.x = 0
    initial_pose.position.y = 1
    initial_pose.position.z = 1

    # Spawn the new model #
    model_path = rospkg.RosPack().get_path('sensor_stick')+'/models/'
    model_xml = ''

    with open (model_path + model_name + '/model.sdf', 'r') as xml_file:
        model_xml = xml_file.read().replace('\n', '')

    spawn_model_prox = rospy.ServiceProxy('gazebo/spawn_sdf_model', SpawnModel)
    spawn_model_prox('training_model', model_xml, '', initial_pose, 'world') 

def __init__(self):
		self._behavior_started = False
		self._preempt_requested = False
		self._current_state = None
		self._active_behavior_id = None

		self._pub = rospy.Publisher('flexbe/start_behavior', BehaviorSelection, queue_size=100)
		self._preempt_pub = rospy.Publisher('flexbe/command/preempt', Empty, queue_size=100)
		self._status_sub = rospy.Subscriber('flexbe/status', BEStatus, self._status_cb)
		self._state_sub = rospy.Subscriber('flexbe/behavior_update', String, self._state_cb)

		self._as = actionlib.SimpleActionServer('flexbe/execute_behavior', BehaviorExecutionAction, None, False)
		self._as.register_preempt_callback(self._preempt_cb)
		self._as.register_goal_callback(self._goal_cb)

		self._rp = RosPack()
		self._behavior_lib = BehaviorLibrary()

		# start action server after all member variables have been initialized
		self._as.start()

		rospy.loginfo("%d behaviors available, ready for start request." % self._behavior_lib.count_behaviors()) 

def __init__(self):
		Logger.initialize()

		self._ready_event = threading.Event()

		self._sub = rospy.Subscriber("flexbe/request_behavior", BehaviorRequest, self._callback)
		self._version_sub = rospy.Subscriber("flexbe/ui_version", String, self._version_callback)

		self._pub = rospy.Publisher("flexbe/start_behavior", BehaviorSelection, queue_size=100)
		self._status_pub = rospy.Publisher("flexbe/status", BEStatus, queue_size=100)
		self._status_sub = rospy.Subscriber("flexbe/status", BEStatus, self._status_callback)
		self._mirror_pub = rospy.Publisher("flexbe/mirror/structure", ContainerStructure, queue_size=100)

		self._rp = RosPack()
		self._behavior_lib = BehaviorLibrary()

		rospy.loginfo("%d behaviors available, ready for start request." % self._behavior_lib.count_behaviors()) 

def test_face_properties():
    local_path = "/tmp/age_gender_weights.hdf5"

    if not os.path.exists(local_path):
        http_path = "https://github.com/tue-robotics/image_recognition/releases/download/" \
                    "image_recognition_keras_face_properties_weights.28-3.73/" \
                    "image_recognition_keras_face_properties_weights.28-3.73.hdf5"
        urllib.urlretrieve(http_path, local_path)
        print("Downloaded weights to {}".format(local_path))

    def age_is_female_from_asset_name(asset_name):
        age_str, gender_str = re.search("age_(\d+)_gender_(\w+)", asset_name).groups()
        return int(age_str), gender_str == "female"

    assets_path = os.path.join(rospkg.RosPack().get_path("image_recognition_keras"), 'test/assets')
    images_gt = [(cv2.imread(os.path.join(assets_path, asset)), age_is_female_from_asset_name(asset))
                 for asset in os.listdir(assets_path)]

    estimations = AgeGenderEstimator(local_path, 64, 16, 8).estimate([image for image, _ in images_gt])
    for (_, (age_gt, is_female_gt)), (age, gender) in zip(images_gt, estimations):
        age = int(age)
        is_female = gender[0] > 0.5
        assert abs(age - age_gt) < 5
        assert is_female == is_female_gt 

def test_face_properties():
    test_key = '69efefc20c7f42d8af1f2646ce6742ec'
    test_secret = '5fab420ca6cf4ff28e7780efcffadb6c'

    def age_is_female_from_asset_name(asset_name):
        age_str, gender_str = re.search("age_(\d+)_gender_(\w+)", asset_name).groups()
        return int(age_str), gender_str == "female"

    assets_path = os.path.join(rospkg.RosPack().get_path("image_recognition_skybiometry"), 'test/assets')
    images_gt = [(cv2.imread(os.path.join(assets_path, asset)), age_is_female_from_asset_name(asset))
                 for asset in os.listdir(assets_path)]

    estimations = Skybiometry(test_key, test_secret).get_face_properties([image for image, _ in images_gt], 10.0)
    for (_, (age_gt, is_female_gt)), face_property in zip(images_gt, estimations):
        age = int(face_property.age_est.value)
        is_female = face_property.gender.value == "female"
        # assert abs(age - age_gt) <= 5  # Poor performance
        assert is_female == is_female_gt 

def _setup(self):
        '''
        Create the mug at a random position on the ground, handle facing
        roughly towards the robot. Robot's job is to grab and lift.
        '''

        rospack = rospkg.RosPack()
        path = rospack.get_path('costar_objects')
        sdf_dir = os.path.join(path, self.sdf_dir)
        obj_to_add = os.path.join(sdf_dir, self.model, self.model_file_name)

        identity_orientation = pb.getQuaternionFromEuler([0, 0, 0])
        try:
            obj_id_list = pb.loadSDF(obj_to_add)
            for obj_id in obj_id_list:
                random_position = np.random.rand(
                    3) * self.spawn_pos_delta + self.spawn_pos_min
                pb.resetBasePositionAndOrientation(
                    obj_id, random_position, identity_orientation)
        except Exception, e:
            print e 

def read_cfg(self, cfg_filename):
        """
        Reads the configuration file

        :param cfg_filename: configuration file name for ORB-SLAM2

        :type cfg_filename: string

        :return: configurations in the configuration file
        :rtype: dict
        """
        rospack = rospkg.RosPack()
        slam_pkg_path = rospack.get_path("orb_slam2_ros")
        cfg_path = os.path.join(slam_pkg_path, "cfg", cfg_filename)
        with open(cfg_path, "r") as f:
            for i in range(1):
                f.readline()
            cfg_data = yaml.load(f, Loader=yaml.FullLoader)
        return cfg_data 

def read_cfg(self, cfg_filename):
        """
        Reads the configuration file

        :param cfg_filename: configuration file name for ORB-SLAM2

        :type cfg_filename: string

        :return: configurations in the configuration file
        :rtype: dict
        """
        rospack = rospkg.RosPack()
        slam_pkg_path = rospack.get_path("orb_slam2_ros")
        cfg_path = os.path.join(slam_pkg_path, "cfg", cfg_filename)
        with open(cfg_path, "r") as f:
            for i in range(1):
                f.readline()
            cfg_data = yaml.load(f)
        return cfg_data 

def _setup(self):
        '''
        Create the mug at a random position on the ground, handle facing
        roughly towards the robot. Robot's job is to grab and lift.
        '''

        rospack = rospkg.RosPack()
        path = rospack.get_path('costar_simulation')
        urdf_dir = os.path.join(path, self.urdf_dir)
        tray_filename = os.path.join(urdf_dir, self.tray_dir, self.tray_urdf)
        red_filename = os.path.join(urdf_dir, self.model, self.red_urdf)
        blue_filename = os.path.join(urdf_dir, self.model, self.blue_urdf)

        for position in self.tray_poses:
            obj_id = pb.loadURDF(tray_filename)
            pb.resetBasePositionAndOrientation(obj_id, position, (0, 0, 0, 1))

        self._add_balls(self.num_red, red_filename, "red")
        self._add_balls(self.num_blue, blue_filename, "blue") 

def running_times():
    rospack = rospkg.RosPack()
    data_path = os.path.join(rospack.get_path('vslam_evaluation'), 'out')
    df = pd.read_csv(os.path.join(data_path, 'runtimes.txt'),
        header=None,
        index_col=0)

    bars = []

    for col_idx in df:
        this_stack = df[col_idx].dropna()
        bars.append(
            go.Bar(
                x=this_stack.index,
                y=this_stack.values,
                name='Thread {}'.format(col_idx)))

    layout = go.Layout(
        barmode='stack',
        yaxis={'title': 'Running time [s]'})

    fig = go.Figure(data=bars, layout=layout)

    url = py.plot(fig, filename='vslam_eval_run_times') 

def __init__(self, config_name, srdf_file, move_group = "", robot_name = ""):
		'''
		Constructor
		'''
		super(GetJointsFromSrdfState, self).__init__(outcomes=['retrieved', 'file_error'],
												output_keys=['joint_values'])

		self._config_name = config_name
		self._move_group = move_group
		self._robot_name = robot_name

		# Check existence of SRDF file to reduce risk of runtime failure.
		# Anyways, values will only be read during runtime to allow modifications.
		self._srdf_path = None
		try:
			rp = RosPack()
			pkg_name = srdf_file.split('/')[0]
			self._srdf_path = os.path.join(rp.get_path(pkg_name), '/'.join(srdf_file.split('/')[1:]))
			if not os.path.isfile(self._srdf_path):
				raise IOError('File "%s" does not exist!' % self._srdf_path)
		except Exception as e:
			Logger.logwarn('Unable to find given SRDF file: %s\n%s' % (srdf_file, str(e)))

		self._file_error = False
		self._srdf = None 

def resolve_filepath(base_path, file_path):
    parsed_url = urlparse(file_path)

    if rospkg and parsed_url.scheme == 'package':
        try:
            rospack = rospkg.RosPack()
            ros_package = parsed_url.netloc
            package_path = rospack.get_path(ros_package)
            resolve_filepath = package_path + parsed_url.path
            if os.path.exists(resolve_filepath):
                return resolve_filepath
        except rospkg.common.ResourceNotFound:
            pass

    dirname = base_path
    file_path = parsed_url.netloc + parsed_url.path
    while not dirname == '/':
        resolved_filepath = os.path.join(dirname, file_path)
        if os.path.exists(resolved_filepath):
            return resolved_filepath
        dirname = os.path.dirname(dirname) 

def load(self):
        '''
        This is an example of a function that allows you to load a robot from
        file based on command line arguments. It just needs to find the
        appropriate directory, use xacro to create a temporary robot urdf,
        and then load that urdf with PyBullet.
        '''

        rospack = rospkg.RosPack()
        path = rospack.get_path('costar_simulation')
        filename = os.path.join(path, self.xacro_filename)
        urdf_filename = os.path.join(path, 'robot', self.urdf_filename)
        urdf = open(urdf_filename, "w")

        # Recompile the URDF to make sure it's up to date
        subprocess.call(['rosrun', 'xacro', 'xacro.py', filename], stdout=urdf)

        self.handle = pb.loadURDF(urdf_filename)
        self.grasp_idx = self.findGraspFrame()
        #self.loadKinematicsFromURDF(urdf_filename, "base_link")

        return self.handle 

def load(self):
        '''
        This is an example of a function that allows you to load a robot from
        file based on command line arguments. It just needs to find the
        appropriate directory, use xacro to create a temporary robot urdf,
        and then load that urdf with PyBullet.
        '''

        rospack = rospkg.RosPack()
        path = rospack.get_path('costar_simulation')
        filename = os.path.join(path, self.xacro_filename)
        urdf_filename = os.path.join(path, 'robot', self.urdf_filename)
        urdf = open(urdf_filename, "w")

        # Recompile the URDF to make sure it's up to date
        subprocess.call(['rosrun', 'xacro', 'xacro.py', filename], stdout=urdf)

        self.handle = pb.loadURDF(urdf_filename)

        return self.handle 

def load(self):
        '''
        This is an example of a function that allows you to load a robot from
        file based on command line arguments. It just needs to find the
        appropriate directory, use xacro to create a temporary robot urdf,
        and then load that urdf with PyBullet.
        '''

        rospack = rospkg.RosPack()
        path = rospack.get_path('costar_simulation')
        filename = os.path.join(path, self.xacro_filename)
        urdf_filename = os.path.join(path, 'robot', self.urdf_filename)
        urdf = open(urdf_filename, "w")

        # Recompile the URDF to make sure it's up to date
        subprocess.call(['rosrun', 'xacro', 'xacro.py', filename], stdout=urdf)

        self.handle = pb.loadURDF(urdf_filename)
        self.grasp_idx = self.findGraspFrame()
        self.loadKinematicsFromURDF(urdf_filename, "base_link")

        return self.handle 

def __init__(self):
		self._rp = RosPack()
		self._behavior_lib = dict()
		self.parse_packages() 

def __init__(self, bagfile=None):
        self._bag = None

        if bagfile is not None:
            bagpath = ''
            # absolute path
            if bagfile.startswith('~') or bagfile.startswith('/'):
                bagpath = os.path.expanduser(bagfile)
            # package-relative path
            else:
                rp = rospkg.RosPack()
                pkgpath = rp.get_path(bagfile.split('/')[0])
                bagpath = os.path.join(pkgpath, '/'.join(bagfile.split('/')[1:]))
            self._bag = rosbag.Bag(bagpath)
            Logger.print_positive('using data source: %s' % bagpath) 

def _setup(self):
        '''
        Create random objects at random positions. Load random objects from the
        scene and create them in different places. In the future we may want to
        switch from using the list of "all" objects to a subset that we can
        actually pick up and manipulate.
        '''

        rospack = rospkg.RosPack()
        path = rospack.get_path('costar_objects')
        sdf_dir = os.path.join(path, self.sdf_dir)
        objs = [obj for obj in os.listdir(
            sdf_dir) if os.path.isdir(os.path.join(sdf_dir, obj))]

        randn = np.random.randint(1, len(objs))

        objs_name_to_add = np.random.choice(objs, randn)
        objs_to_add = [os.path.join(sdf_dir, obj, self.model_file_name)
                       for obj in objs_name_to_add]

        identity_orientation = pb.getQuaternionFromEuler([0, 0, 0])
        # load sdfs for all objects and initialize positions
        for obj_index, obj in enumerate(objs_to_add):
            if objs_name_to_add[obj_index] in self.models:
                try:
                    print 'Loading object: ', obj
                    obj_id_list = pb.loadSDF(obj)
                    for obj_id in obj_id_list:
                        self.objs.append(obj_id)
                        random_position = np.random.rand(
                            3) * self.spawn_pos_delta + self.spawn_pos_min
                        pb.resetBasePositionAndOrientation(
                            obj_id, random_position, identity_orientation)
                except Exception, e:
                    print e 

def __init__(self, launch_config, wait_cond="True"):
        self._run_id = rospy.get_param('/run_id')
        launchpath = None
        launchcontent = None

        # load from system path
        if launch_config.startswith('~') or launch_config.startswith('/'):
            launchpath = os.path.expanduser(launch_config)
        # load from package path
        elif re.match(r'.+\.launch$', launch_config):
            rp = rospkg.RosPack()
            pkgpath = rp.get_path(launch_config.split('/')[0])
            launchpath = os.path.join(pkgpath, '/'.join(launch_config.split('/')[1:]))
        # load from config definition
        else:
            launchcontent = launch_config

        launchconfig = roslaunch.config.ROSLaunchConfig()
        loader = roslaunch.xmlloader.XmlLoader()
        if launchpath is not None:
            loader.load(launchpath, launchconfig, verbose=False)
        else:
            loader.load_string(launchcontent, launchconfig, verbose=False)
        self._launchrunner = roslaunch.launch.ROSLaunchRunner(self._run_id, launchconfig)
        self._wait_cond = wait_cond
        self._valid = True 

def start(self):
        self.setup_report_file()

        for i, settings in enumerate(self.config['repetitions']):
            print "STARITNG REPETITION: ", i
            path = rospkg.RosPack().get_path("fssim") + "/scripts/automated_res.py"
            launching_script = "python {} --config {} --id {} --output {} ".format(path, self.args.config, i, self.args.output)
            print launching_script
            self.fssim_cmd = Command(launching_script)
            self.fssim_cmd.run()
            self.fssim_cmd.join()
            time.sleep(5.0)

        print "EXITING LAUNCHER SCRIPT"
        sys.exit(0) 

def generate_track_model(track_name):
    """
    Hacky way of changing track in gazebo
    :param track_name:
    """
    # type: (str) -> None

    rospack = rospkg.RosPack()
    fssim_gazebo = rospack.get_path('fssim_gazebo')
    xacro_file = fssim_gazebo + '/models/track/model.xacro'
    xacro_out_file = fssim_gazebo + '/models/track/model.config'
    command = "xacro --inorder {} track_name:={} > {}".format(xacro_file, track_name, xacro_out_file)
    xacro_proccess = subprocess.check_output(command, shell = True, stderr = subprocess.STDOUT) 

def parse_path(path, pkg_config_storage):
    '''
    Parses path based on whether it is global path or wrt to pkg_pkg_config_storage
    :param path:
    :param pkg_config_storage:
    :return: path to string
    '''
    # type: (str, str) -> str
    if path[0] == '/':  # Global path
        return path
    else:  # Path wrt pkg_config_storage
        return rospkg.RosPack().get_path(pkg_config_storage) + "/" + path 

def __init__(self, arm, epsilon_ok=0.03, with_orientation=True, min_num_demos=3, std_factor=2, dataset_id=-1):
        """
        :param arm: string ID of the FK/IK group (left, right, ...)
        :param epsilon_ok: maximum acceptable cartesian distance to the goal
        :param with_orientation: True for context = position + orientation, False for context = position only
        :param min_num_demos: Minimum number of demos per primitive
        :param std_factor: Factor applied to the cartesian standard deviation so within this range, the MP is valid
        :param dataset_id: ID of the dataset to work with, id < 0 will create a new one
        """
        rospack = rospkg.RosPack()
        self.path_ds = join(rospack.get_path('prompros'), 'datasets')
        self.path_plots = join(rospack.get_path('prompros'), 'plots')
        super(ReplayableInteractiveProMP, self).__init__(arm, epsilon_ok, with_orientation, min_num_demos, std_factor, self.path_ds, dataset_id, self.path_plots)
        self._durations = []
        self.joint_names = [] 

def __init__(self, context):
        """
        Constructor
        """
        super(CarlaControlPlugin, self).__init__(context)
        self.setObjectName('CARLA Control')

        self._widget = QWidget()
        ui_file = os.path.join(rospkg.RosPack().get_path(
            'rqt_carla_control'), 'resource', 'CarlaControl.ui')
        loadUi(ui_file, self._widget)
        self._widget.setObjectName('CarlaControl')
        if context.serial_number() > 1:
            self._widget.setWindowTitle(
                self._widget.windowTitle() + (' (%d)' % context.serial_number()))

        self.pause_icon = QIcon(
            QPixmap(os.path.join(
                rospkg.RosPack().get_path('rqt_carla_control'), 'resource', 'pause.png')))
        self.play_icon = QIcon(
            QPixmap(os.path.join(
                rospkg.RosPack().get_path('rqt_carla_control'), 'resource', 'play.png')))
        self._widget.pushButtonStepOnce.setIcon(
            QIcon(QPixmap(os.path.join(
                rospkg.RosPack().get_path('rqt_carla_control'), 'resource', 'step_once.png'))))

        self.carla_status = None
        self.carla_status_subscriber = rospy.Subscriber(
            "/carla/status", CarlaStatus, self.carla_status_changed)
        self.carla_control_publisher = rospy.Publisher(
            "/carla/control", CarlaControl, queue_size=10)

        self._widget.pushButtonPlayPause.setDisabled(True)
        self._widget.pushButtonStepOnce.setDisabled(True)
        self._widget.pushButtonPlayPause.setIcon(self.play_icon)
        self._widget.pushButtonPlayPause.clicked.connect(self.toggle_play_pause)
        self._widget.pushButtonStepOnce.clicked.connect(self.step_once)

        context.add_widget(self._widget) 

def __init__(self):
    print('constructor')
    # Init QWidget
    super(CalibWidget, self).__init__()
    self.setObjectName('CalibWidget')

    # load UI
    ui_file = os.path.join(rospkg.RosPack().get_path('dvs_calibration_gui'), 'resource', 'widget.ui')
    loadUi(ui_file, self)

    # init and start update timer for data, the timer calls the function update_info all 40ms    
    self._update_info_timer = QTimer(self)
    self._update_info_timer.timeout.connect(self.update_info)
    self._update_info_timer.start(40)

    self.button_reset.pressed.connect(self.on_button_reset_pressed)
    self.button_start.pressed.connect(self.on_button_start_calibration_pressed)
    self.button_save.pressed.connect(self.on_button_save_calibration_pressed)
    
    self._sub_pattern_detections = rospy.Subscriber('dvs_calibration/pattern_detections', Int32, self.pattern_detections_cb)    
    self._sub_calibration_output = rospy.Subscriber('dvs_calibration/output', String, self.calibration_output_cb)

    print('reset')

    self.on_button_reset_pressed()
    
    print('reset done') 

def __init__(self,faultlog_array):
               
        """
        If this path does not exists make it it is the default path for the
        faultlog extraction
        """
        if (False == os.path.exists(os.getcwd()+"/SI_FAULTLOGS/img")):
            os.makedirs(os.getcwd()+"/SI_FAULTLOGS/img")
        
        """
        Get the directory and make sure the user did not cancel
        """
        self.dir_path = os.getcwd()+ "/SI_FAULTLOGS"
        
        """
        Copy the logo over to the directory
        """
        rospack = rospkg.RosPack()
        img = rospack.get_path('movo_ros') + "/src/movo/"
        shutil.copyfile(img+'logo.png',os.getcwd()+"/SI_FAULTLOGS/img/logo.png") 
        
        """
        Parse the faultlog array
        """
        filename = self.dir_path + "/" + "SI_FAULTOG_" + time.strftime("%m%d%Y_%H%M%S") + ".html"     
        Create_Log_File(filename,faultlog_array.data)
        try:
            webbrowser.open(filename)
        except:
            webbrowser.open(filename) 

def getPaths():
    # looks up gps file logging location
    global location_collect
    rospack = rospkg.RosPack()
    
    location_collect = rospack.get_path('lawn_tractor_navigation') + "/gps_points_file.txt" 

def load(self):
        '''
        This is an example of a function that allows you to load a robot from
        file based on command line arguments. It just needs to find the
        appropriate directory, use xacro to create a temporary robot urdf,
        and then load that urdf with PyBullet.
        '''

        rospack = rospkg.RosPack()
        path = rospack.get_path('costar_simulation')
        filename = os.path.join(path, self.xacro_filename)
        urdf_filename = os.path.join(path, 'robot', self.urdf_filename)
        urdf = open(urdf_filename, "r")
        # urdf =
        # open("/home/albert/costar_ws/src/costar_plan/costar_simulation/robot/jaco_robot.urdf",
        # "r")

        # Recompile the URDF to make sure it's up to date
        # subprocess.call(['rosrun', 'xacro', 'xacro.py', filename],
        # stdout=urdf)

        self.handle = pb.loadURDF(urdf_filename)
        self.grasp_idx = self.findGraspFrame()
        self.loadKinematicsFromURDF(urdf_filename, "robot_root")

        return self.handle 

def test_head_debug():
    """
    Test the blob detection using images on disk
    """

    # Get files
    path = rospkg.RosPack().get_path('sorting_demo') + "/share/test_head_simulator"
    files = [f for f in os.listdir(path) if os.path.isfile(os.path.join(path, f))]
    #print(files)

    # Process files
    for f in files:
        # Get image path
        image_path = os.path.join(path, f)
        print(image_path)

        # Read image
        cv_image = cv2.imread(image_path)

        # Get color blobs info
        blob_info = get_blobs_info(cv_image)
        print(blob_info)

        # Wait for a key press
        cv2.waitKey(0)

    # Exit
    cv2.destroyAllWindows()