Пример #1
0
def get_pkg_dir_from_prefix(path, ros_package_path=None):
    
    if RE_PREFIXED_TAG not in path:
        return path
    
    splitpath = path.split(RE_PREFIXED_END_TAG)
    
    after_prefix_path = splitpath[-1]
    
    prefix = splitpath[0].split(RE_PREFIXED_BEGIN_TAG)[-1]
    
    if prefix == DEFAULT_PREFIX_NAME:
        if ros_package_path is not None:
            rpath = ros_package_path+after_prefix_path
        else:
            raise ROSPkgException("Unknown prefixed ${prefix} path ! not associated to ros package path !")
    elif prefix == ROS_WS_PREFIX_NAME:
        rpath = get_ros_workspace_dir()+after_prefix_path
    elif prefix == ROS_WS_SRC_PREFIX_NAME:
        print get_ros_workspace_src_dir(),after_prefix_path
        rpath = get_ros_workspace_src_dir()+after_prefix_path
    else:	
        rpath = get_pkg_dir(prefix)+after_prefix_path
    
    return rpath
    def setDirectoryNames(self):

        """
        Sets the dictionary database movement dancing reem structure.
        """

        base = packages.get_pkg_dir(PACKAGE_WHERE_TO_FIND_DANCING_MOVEMENTS)
        dbdir = os.path.join(base, DANCING_DATABASE_DIR)

        self.DANGEROUS_DIR = os.path.join(dbdir, 'dangerous')
        self.MIDDLE_DIR = os.path.join(dbdir, 'middle')
        self.UP_DIR = os.path.join(dbdir, 'up')
        self.SIDES_DIR = os.path.join(dbdir, 'sides')
        self.INITIAL_DIR = os.path.join(dbdir, 'initial')

        self.listOfDirectories = {'dangerous':  self.DANGEROUS_DIR,
                                  'middle':     self.MIDDLE_DIR,
                                  'up':         self.UP_DIR,
                                  'sides':      self.SIDES_DIR,
                                  'initial':    self.INITIAL_DIR}

        self.movements = {'dangerous':        filter(lambda x: x.endswith('.xml'), os.listdir(self.DANGEROUS_DIR)),
                          'middle':       filter(lambda x: x.endswith('.xml'), os.listdir(self.MIDDLE_DIR)),
                          'up':      filter(lambda x: x.endswith('.xml'), os.listdir(self.UP_DIR)),
                          'sides':           filter(lambda x: x.endswith('.xml'), os.listdir(self.SIDES_DIR)),
                          'initial':          filter(lambda x: x.endswith('.xml'), os.listdir(self.INITIAL_DIR))}

        self.previousMov = ''
Пример #3
0
 def resolve(self, string, arg_dict):
     """Resolves $() substitutions in the roslaunch 
     """
     original = string
     regex = r'\$\(([^)]+)\)'
     match = re.search(regex, string)
     while match is not None:
         full_match = match.group(0)
         m = match.group(1)
         keyword = m.split(' ')[0]
         substitution = full_match.replace('$(', '$_(') #default substitution
         if keyword == 'arg':
             arg_name = m.split(' ')[1]
             if arg_name in arg_dict:
                 substitution = arg_dict[arg_name]
             else:
                 print 'Argument %s not set' % arg_name
         elif keyword == 'env':
             env_variable = m.split(' ')[1]
             if env_variable in os.environ:
                 substitution = os.environ[env_variable]
             else:
                 print 'Environment variable %s not set' % env_variable
         elif keyword == 'find':
             package_name = m.split(' ')[1]
             try:
                 package_path = get_pkg_dir(package_name)
                 substitution = package_path
             except InvalidROSPkgException:
                 print 'Could not find pacakage %s' % package_name 
         string = string.replace(full_match, substitution)
         match = re.search(regex, string)            
     return {'resolved':string, 'variable':original}
Пример #4
0
    def __init__(self, pgm_grasp_type=True):
        self.pgm_grasp_type = pgm_grasp_type

        self.isrr_limit = False  #True
        self.hand_config_frame_id = None

        pkg_path = rp.get_pkg_dir('prob_grasp_planner')

        self.pca_model_path = pkg_path + '/train_models_sim/pca/pca.model'
        self.train_model_path = pkg_path + '/train_models_sim/classifiers/'
        self.prior_path = pkg_path + '/train_models_sim/priors/'

        self.non_type_prior = True
        self.load_learned_models()

        #Use the computer tars or t
        self.tars = True  #False
        if self.tars:
            self.grasp_config_log_save_path = '/media/kai/logs/multi_finger_sim_data/grad_des_ls_log/'
        else:
            self.grasp_config_log_save_path = '/dataspace/data_kai/logs/multi_finger_sim_data/grad_des_ls_log/'
        self.iter_total_num = 1000  #500

        self.log_inf = True
        #The regularization value is related with both the
        #data (voxel, config) dimension and the number of GMM components.
        self.reg_log_prior = 0.1  #1.
        #self.reg_log_prior = 0.01
        #self.reg_log_prior = 0.05
        #Regularization for log likelihood, this is mainly used to
        #test the inference with only prior.
        self.reg_log_lkh = 1.

        self.q_max = 2.
        self.q_min = -2.
def create_ikfast_package(args):
  try:
    setattr(args, 'ikfast_plugin_pkg_path', get_pkg_dir(args.ikfast_plugin_pkg))
  except InvalidROSPkgException:
    args.ikfast_plugin_pkg_path = os.path.abspath(args.ikfast_plugin_pkg)
    print("Failed to find package: %s. Will create it in %s." %
          (args.ikfast_plugin_pkg, args.ikfast_plugin_pkg_path))
    # update pkg name to basename of path
    args.ikfast_plugin_pkg=os.path.basename(args.ikfast_plugin_pkg_path)

  src_path = args.ikfast_plugin_pkg_path + '/src/'
  if not os.path.exists(src_path):
     os.makedirs(src_path)

  include_path = args.ikfast_plugin_pkg_path + '/include/'
  if not os.path.exists(include_path):
     os.makedirs(include_path)

  # Create package.xml
  pkg_xml_path = args.ikfast_plugin_pkg_path + '/package.xml'
  if not os.path.exists(pkg_xml_path):
    root = xmlElement("package", format="2")
    root.append(xmlElement("name", text=args.ikfast_plugin_pkg))
    root.append(xmlElement("version", text="0.0.0"))
    root.append(xmlElement("description", text="IKFast plugin for " + args.robot_name))
    root.append(xmlElement("license", text="BSD"))
    user_name = getuser()
    root.append(xmlElement("maintainer", email="*****@*****.**" % user_name, text=user_name))
    root.append(xmlElement("buildtool_depend", text="catkin"))
    with open(pkg_xml_path, 'w') as f:
      etree.ElementTree(root).write(f, xml_declaration=True, pretty_print=True, encoding="UTF-8")
    print("Created package.xml at: '%s'" % pkg_xml_path)
Пример #6
0
def create_ikfast_package(args):
  try:
    setattr(args, 'ikfast_plugin_pkg_path', get_pkg_dir(args.ikfast_plugin_pkg))
  except InvalidROSPkgException:
    args.ikfast_plugin_pkg_path = os.path.abspath(args.ikfast_plugin_pkg)
    print("Failed to find package: %s. Will create it in %s." %
          (args.ikfast_plugin_pkg, args.ikfast_plugin_pkg_path))
    # update pkg name to basename of path
    args.ikfast_plugin_pkg=os.path.basename(args.ikfast_plugin_pkg_path)

  src_path = args.ikfast_plugin_pkg_path + '/src/'
  if not os.path.exists(src_path):
     os.makedirs(src_path)

  include_path = args.ikfast_plugin_pkg_path + '/include/'
  if not os.path.exists(include_path):
     os.makedirs(include_path)

  # Create package.xml
  pkg_xml_path = args.ikfast_plugin_pkg_path + '/package.xml'
  if not os.path.exists(pkg_xml_path):
    root = xmlElement("package", format="2")
    root.append(xmlElement("name", text=args.ikfast_plugin_pkg))
    root.append(xmlElement("version", text="0.0.0"))
    root.append(xmlElement("description", text="IKFast plugin for " + args.robot_name))
    root.append(xmlElement("license", text="BSD"))
    user_name = getuser()
    root.append(xmlElement("maintainer", email="*****@*****.**" % user_name, text=user_name))
    root.append(xmlElement("buildtool_depend", text="catkin"))
    with open(pkg_xml_path, 'w') as f:
      etree.ElementTree(root).write(f, xml_declaration=True, pretty_print=True, encoding="UTF-8")
    print("Created package.xml at: '%s'" % pkg_xml_path)
Пример #7
0
def SelectRandomMP3FileFromMp3Lib():

    """
    Selects radomly a song in the mp3_lib dir in the dancing_reem package.
    It now take mp3 files and wav files. We dont get .ogg because soundstretch doesnt.
    """

    base = packages.get_pkg_dir(DANCING_REEM_PKG)
    path_to_mp3_lib = os.path.join(base, NAME_MP3_LIBRARY_DIR)

    list_mp3_songs_in_mp3lib = filter(lambda x: x.endswith('.mp3'), os.listdir(path_to_mp3_lib))
    list_wav_songs_in_mp3lib = filter(lambda x: x.endswith('.wav'), os.listdir(path_to_mp3_lib))
    list_songs_in_mp3lib = list_mp3_songs_in_mp3lib + list_wav_songs_in_mp3lib

    print "Song LIST" + str(list_songs_in_mp3lib)

    if len(list_songs_in_mp3lib) == 0:
        rospy.loginfo("###### The MP3 Library is EMPTY ######")
        song_path = 'None'
    else:
        random_song_name = list_songs_in_mp3lib[randint(0, len(list_songs_in_mp3lib) - 1)]
        print "SELECTED SONG: " + str(random_song_name)
        song_path = os.path.join(path_to_mp3_lib, random_song_name)

    return song_path
Пример #8
0
    def __init__(self, node_uri, node_name):

        rsc = os.path.join(get_pkg_dir('airbus_plugin_node_manager'),
                           'resources')

        self._icon_node_start = QIcon(rsc + '/start.png')
        self._icon_node_stop = QIcon(rsc + '/stop.png')

        self.uri = QLabel(node_uri)
        self.uri.setContentsMargins(0, 0, 10, 0)
        self.uri.setMinimumHeight(40)

        self.name = QLabel(node_name)
        self.name.setContentsMargins(0, 0, 10, 0)
        self.name.setMinimumHeight(40)

        self.status = QLabel(self.RUNNING)
        self.status.setStyleSheet('qproperty-alignment: AlignCenter;')
        self.status.setMinimumSize(QSize(100, 40))

        self.ping = QLabel('...')
        self.ping.setStyleSheet("qproperty-alignment: AlignCenter;")

        self.button_start_stop = QPushButton()
        self.button_start_stop.setIcon(self._icon_node_stop)
        self.button_start_stop.setIconSize(QSize(30, 30))
        self.button_start_stop.setFixedSize(QSize(100, 40))

        self.button_start_stop_widget = widget_creator(self.button_start_stop)

        if node_name not in self.NODE_EXCEPTION:
            self.button_start_stop.clicked.connect(self.start_stop_slot)
            self.button_start_stop.setEnabled(False)

        self.current_status = self.NONE
Пример #9
0
def cmd_create_launch(data_central, argv):
    '''Creates ROS launch files for all combinations of agents and robots.'''
    parser = OptionParser(prog='create-launch',
                          usage=cmd_create_launch.short_usage)
    parser.disable_interspersed_args()
    parser.add_option("-a", "--agent", dest='id_agent', help="Agent ID")
    parser.add_option("-r", "--robot", dest='id_robot', help="Robot ID")

    parser.add_option('-p', '--package', dest='package',
                      default='bootstrapping_adapter',
            help='Which ROS package to put the launch file in [%default].')

    # TODO: bag
    (options, args) = parser.parse_args(argv)

    check_no_spurious(args)

    bo_config = data_central.get_bo_config()

    if options.id_agent is None:
        id_agents = bo_config.agents.keys()
    else:
        id_agents = [options.id_agent] # TODO: expand

    if options.id_robot is None:
        id_robots = bo_config.robots.keys()
    else:
        id_robots = [options.id_robot] # TODO: expand

    for id_agent in id_agents:
        check_contained(id_agent, bo_config.agents)
    for id_robot in id_robots:
        check_contained(id_robot, bo_config.robots)

    from roslib import packages #@UnresolvedImport
    out = packages.get_pkg_dir(options.package)
    outdir = os.path.join(out, 'launch', 'boot_olympics')

    root = os.path.realpath(data_central.root)
    for id_robot, id_agent in itertools.product(id_robots, id_agents):
        robot_ros_node = wrap_python_robot(bo_config.robots[id_robot], root)
        agent_ros_node = wrap_python_agent(bo_config.agents[id_agent], root)
        xml = create_launch_xml(agent_ros_node=agent_ros_node,
                                robot_ros_node=robot_ros_node,
                                agent_node_id='my_agent',
                                robot_node_id='my_robot',
                                namespace='boot_olympics',
                                bag=None,
                                output=None)
        basename = '%s-%s' % (id_robot, id_agent)

        filename = os.path.join(outdir, '%s.launch' % basename)

        make_sure_dir_exists(filename)
        with open(filename, 'w') as f:
            f.write('<!-- Created by %s on %s -->\n' %
                    ('boot_olympics', isodate()))
            f.write(xml)

        logger.info('Writing on %r.' % filename)
Пример #10
0
    def __init__(self):
        super(WalkingModule, self).__init__(module_name='walking_module')

        self.is_initial_walk = False

        # Initialize OP3 ROS publishers
        self.set_walking_command_pub = rospy.Publisher(
            '/robotis/walking/command', String, queue_size=0)
        self.set_walking_param_pub = rospy.Publisher(
            '/robotis/walking/set_params', WalkingParam, queue_size=0)
        self.module_control_pub = rospy.Publisher(
            '/robotis/enable_ctrl_module', String, queue_size=0)

        # Initialize configuration param variables
        self.current_walking_param = WalkingParam()
        self.initial_param = None
        self.marching_param = None
        self.forward_param = None
        self.backward_param = None
        self.turn_left_param = None
        self.turn_right_param = None

        # Load configurations
        config_file_path = os.path.join(rospkg.get_pkg_dir('obstacle_run'),
                                        'config',
                                        'walking_configurations.json')
        self.load_configuration(file_path=config_file_path)
Пример #11
0
    def __init__(self):
        rospy.init_node("declination_provider")
        wmm_path = path.join(get_pkg_dir('declination'), "wmm", "WMM.COF")
        self.gm = GeoMag(wmm_path)

        self.fix = None
        self.sub = rospy.Subscriber("fix", NavSatFix, self._fix)
        self.pub = rospy.Publisher("declination", Float32, latch=True)
def find_template_dir():
  for candidate in [os.path.dirname(__file__) + '/../templates']:
    if os.path.exists(candidate) and os.path.exists(candidate + '/ikfast.h'):
      return os.path.realpath(candidate)
  try:
    return os.path.join(get_pkg_dir(plugin_gen_pkg), 'ikfast_kinematics_plugin/templates')
  except InvalidROSPkgException:
    raise Exception("Can't find package %s" % plugin_gen_pkg)
Пример #13
0
def find_template_dir():
  for candidate in [os.path.dirname(__file__) + '/../templates']:
    if os.path.exists(candidate) and os.path.exists(candidate + '/ikfast.h'):
      return os.path.realpath(candidate)
  try:
    return os.path.join(get_pkg_dir(plugin_gen_pkg), 'ikfast_kinematics_plugin/templates')
  except InvalidROSPkgException:
    raise Exception("Can't find package %s" % plugin_gen_pkg)
Пример #14
0
    def execute(self, userdata):

        absolute_path_mov_name = os.path.join(packages.get_pkg_dir(NAME_OF_PKG_PLAY_BALL_DEMO), 'scripts/stop_demo.sh')
        print str(absolute_path_mov_name)
        assert (os.path.exists(absolute_path_mov_name)), "The path to the stop_demo.sh file doesn't exist"
        absolute_path_mov_name
        rospy.loginfo("STOPPED PLAY_BALL_DEMO.SH ")

        return succeeded
 def initializeCommandVariables(userdata):
     global follow_grammar_name_index
     follow_grammar_name_index = 0
     try:
         global bag
         bag = rosbag.Bag(packages.get_pkg_dir('common') + '/config/getin_odometry.bag', 'w')
     finally:
         debugPrint("Odometry bag opened", 3)
     return succeeded
    def __init__(self):
        # Parameters
        self.grasp_patch_rows = 400
        self.grasp_patch_cols = 400
        self.rgbd_channels = 8
        self.rate_dec_epochs = 1000  #400
        self.training_epochs = 3000  #1200
        self.batch_size = 8  #16
        self.display_step = 10  #0
        self.dropout_prob = 0.75  #0.9
        self.config_dim = 14  #11
        #self.read_data_ratio = 0.25
        #self.read_batch_epochs = 5000
        self.read_data_ratio = 0.5
        self.read_batch_epochs = 5000
        self.classes_num = 1

        #rgbd_patches_save_path = '/data_space/data_kai/multi_finger_exp_data'
        rgbd_patches_save_path = '/mnt/tars_data/multi_finger_sim_data_complete_v4/'
        self.grasp_patches_file_path = rgbd_patches_save_path + 'grasp_patches.h5'
        self.grasp_data_file_path = rgbd_patches_save_path + 'grasp_data.h5'
        self.logs_path = '/home/qingkai/tf_logs/multi_finger_sim_data'
        pkg_path = rp.get_pkg_dir('prob_grasp_planner')

        self.cnn_model_path = pkg_path + '/models/grasp_cnn_planner/' + \
                              'models/cnn_rgbd_config.ckpt'
        self.prior_path = pkg_path + '/models/grasp_cnn_planner/priors/'

        # tf Graph input
        self.holder_grasp_patch = tf.placeholder(tf.float32, [
            None, self.grasp_patch_rows, self.grasp_patch_cols,
            self.rgbd_channels
        ],
                                                 name='holder_grasp_patch')
        self.holder_config = tf.placeholder(tf.float32,
                                            [None, self.config_dim],
                                            name='holder_config')
        self.holder_labels = tf.placeholder(tf.float32,
                                            [None, self.classes_num],
                                            name='holder_labels')
        self.keep_prob = tf.placeholder(tf.float32, name='holder_keep_prob')
        self.learning_rate = tf.placeholder(tf.float32,
                                            name='holder_learn_rate')

        self.train_samples_pct = 0.8
        self.train_samples_num = -1
        self.testing_samples_num = -1
        self.training_labels = None
        self.training_grasp_samples = None
        self.training_samples_fingers = None
        self.testing_labels = None
        self.testing_grasp_samples = None
        self.testing_samples_fingers = None
        self.read_data_batch = False
        self.alpha_ridge = 0.5
        #Need to change this to false for cross validation.
        self.use_all_data_train = True
Пример #17
0
def generateRosMessages(toc):
    """ Generates the *.msg files for ROS from the TOC
    """
    if not toc:
        rospy.logwarn("No TOC available to generate ROS messages from")
        return

    path = get_pkg_dir('crazyflieROS')+"/msg/"
    for g in toc.keys():
        makeMsg(g, path, toc[g] )
Пример #18
0
    def _parse(self, launch_file, digraph):

        launch_name = launch_file.split('/')[-1].split('.')[0]

        root = None
        try:
            root = ElementTree.parse(launch_file)
        except Exception as ex:
            html.HTMLException(ex, self)
            return

        tree = root.getroot()

        for elem in tree:
            #<include file="$(find pma_startup)/launch/pma1_driver.launch" />
            if elem.tag == 'include':

                include_split = elem.attrib["file"].split(")")
                prefix = include_split[0]
                pkg_name = prefix.split(" ")[-1]

                child = include_split[-1].split("/")[-1].split('.')[0]

                n1 = NODE(launch_name)
                n1.setAttrib(NODE.SHAPE, SHAPE.Box)
                n1.setAttrib(NODE.STYLE, STYLE.FILLED)
                n1.setAttrib(NODE.COLOR, RgbColor.LightGray)
                digraph.addNode(n1)

                n2 = NODE(child)
                n2.setAttrib(NODE.SHAPE, SHAPE.Box)
                n2.setAttrib(NODE.STYLE, STYLE.FILLED)
                n2.setAttrib(NODE.COLOR, RgbColor.LightGray)
                digraph.addNode(n2)

                digraph.connect(n1, n2)

                sub_launch = get_pkg_dir(pkg_name) + include_split[-1]

                self._parse(sub_launch, digraph)

            #<node name="map_server_high_res" pkg="map_server" type="map_server" args="$(arg map_file_high_res)" respawn="false">
            elif elem.tag == "node":

                parent = launch_name
                child = Prefix.arg(elem.attrib["name"], tree)
                if parent == child:
                    child += "_node"

                nd = NODE(child)
                nd.setAttrib(NODE.SHAPE, SHAPE.Ellipse)
                nd.setAttrib(NODE.STYLE, STYLE.FILLED)
                nd.setAttrib(NODE.COLOR, RgbColor.LightSeaGreen)
                digraph.addNode(nd)
                digraph.connect(parent, nd)
Пример #19
0
    def create_buttons(self):
        QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
        file_name=rospy.get_param('xml_button_file', None)
        file_name_pkg=rospy.get_param('xml_button_file_pkg', None)
        if file_name==None:
           file_name =unicode(get_pkg_dir("python_gui"))+"/xml/default.xml"
        elif file_name_pkg != None:
           file_name = unicode(get_pkg_dir(file_name_pkg))+file_name
            
        print "file name:\n"+file_name
        file_map = etree.parse(file_name)
        
        list_event =  file_map.getroot()
        grid = QtGui.QGridLayout()
        i=0
        for child  in list_event:
            self.string_map[child.get('name')]=child.get('event')
            btn=QtGui.QPushButton(child.get('name'), self)
            grid.addWidget(btn,i,0)
            btn.clicked.connect(self.buttonClicked) 
            tooltip_text=child.get('tooltip')
            if tooltip_text!=None:
                btn.setToolTip(tooltip_text)
            
            i=i+1
 
        #layout: a vbox divided in 3 parts
        # the grid, a Stretch to fill in the space and a status bar
        
        vbox = QtGui.QVBoxLayout()
        vbox.addLayout(grid)
        self.statBar = QtGui.QStatusBar(self)
        vbox.addStretch(1)
        vbox.addWidget(self.statBar)
        self.statBar.showMessage("Event sender by GB")
        
        self.setLayout(vbox)
        self.setGeometry(300, 300, 290, 150)
        self.move(1500, 150)
        self.setWindowTitle('Event sender')
        self.setWindowIcon(QtGui.QIcon(unicode(get_pkg_dir("python_gui")) + '/resources/es_ico.png'))
        self.show()
Пример #20
0
    def test_params(self):
        mock = self._load(os.path.join(self.xml_dir, 'test-params-valid.xml'))

        param_d = {}
        for p in mock.params:
            param_d[p.key] = p.value

        self.assertEquals('pass', param_d['/override'])
        self.assertEquals('bar2', param_d['/somestring1'])
        self.assertEquals('10', param_d['/somestring2'])
        self.assertEquals(1, param_d['/someinteger1'])
        self.assertEquals(2, param_d['/someinteger2'])
        self.assertAlmostEquals(3.14159, param_d['/somefloat1'], 2)
        self.assertAlmostEquals(5.0, param_d['/somefloat2'], 1)
        self.assertEquals("a child namespace parameter 1",
                          param_d['/wg/wgchildparam'], p.value)
        self.assertEquals("a child namespace parameter 2",
                          param_d['/wg2/wg2childparam1'], p.value)
        self.assertEquals("a child namespace parameter 3",
                          param_d['/wg2/wg2childparam2'], p.value)

        import xmlrpclib
        from roslib.packages import get_pkg_dir
        f = open(os.path.join(get_pkg_dir('roslaunch'), 'example.launch'))
        try:
            contents = f.read()
        finally:
            f.close()
        p = [p for p in mock.params if p.key == '/configfile'][0]
        self.assertEquals(contents, p.value, 1)
        p = [p for p in mock.params if p.key == '/binaryfile'][0]
        self.assertEquals(xmlrpclib.Binary(contents), p.value, 1)

        f = open(
            os.path.join(get_pkg_dir('roslib'), 'scripts',
                         'python-reserved-words.txt'))
        try:
            contents = f.read()
        finally:
            f.close()
        p = [p for p in mock.params if p.key == '/commandoutput'][0]
        self.assertEquals(contents, p.value, 1)
Пример #21
0
def get_pkg_dir_from_prefix(path, ros_package_path=None):
    
    if RE_PREFIXED_TAG not in path:
        return path
    
    splitpath = path.split(RE_PREFIXED_END_TAG)
    after_prefix_path = splitpath[-1]
    prefix = splitpath[0].split(RE_PREFIXED_BEGIN_TAG)[-1]
    rpath = get_pkg_dir(prefix)+after_prefix_path
    
    return rpath
Пример #22
0
    def create_buttons(self):
        QtGui.QToolTip.setFont(QtGui.QFont('SansSerif', 10))
        file_name = rospy.get_param('xml_button_file', None)
        file_name_pkg = rospy.get_param('xml_button_file_pkg', None)
        if file_name == None:
            file_name = unicode(get_pkg_dir("python_gui")) + "/xml/default.xml"
        elif file_name_pkg != None:
            file_name = unicode(get_pkg_dir(file_name_pkg)) + file_name

        print "file name:\n" + file_name
        file_map = etree.parse(file_name)

        list_event = file_map.getroot()
        grid = QtGui.QGridLayout()
        i = 0
        for child in list_event:
            self.string_map[child.get('name')] = child.get('event')
            btn = QtGui.QPushButton(child.get('name'), self)
            grid.addWidget(btn, i, 0)
            btn.clicked.connect(self.buttonClicked)
            tooltip_text = child.get('tooltip')
            if tooltip_text != None:
                btn.setToolTip(tooltip_text)

            i = i + 1

        #layout: a vbox divided in 3 parts
        # the grid, a Stretch to fill in the space and a status bar

        vbox = QtGui.QVBoxLayout()
        vbox.addLayout(grid)
        self.statBar = QtGui.QStatusBar(self)
        vbox.addStretch(1)
        vbox.addWidget(self.statBar)
        self.statBar.showMessage("Event sender by GB")

        self.setLayout(vbox)
        self.setGeometry(300, 300, 290, 150)
        self.move(1500, 150)
        self.setWindowTitle('Event sender')
        self.show()
Пример #23
0
    def parse_find_keyword(self, string):
        match = re.match(r'.*\$\(find (\S+)\).*', string)
        if match is not None:
            package_name = match.group(1)
            try:
                package_path = get_pkg_dir(package_name)
            except InvalidROSPkgException:
                self.add_issue('cannot find package %s' % (package_name))
                return None

            string = re.sub(r'\$\(find \S+\)', package_path, string)
        return string
Пример #24
0
def getStandAloneInstance(pkg_name, dashboard_class, lng="en"):
    
    dashboard_instance = dashboard_class(Context(QMainWindow()))
    
    dashboard_descriptor = ET.parse("%s/dashboard_descriptor.xml"%get_pkg_dir(pkg_name))
    
    dashboard_params = DashboardProvider.getParameters(dashboard_descriptor.getroot(), None)
    
    dashboard_instance.setup(dashboard_descriptor, dashboard_params)
    
    dashboard_instance.onTranslate(lng)
    
    return dashboard_instance
Пример #25
0
 def find(path):
     
     if Prefix.is_prefixed(path):
         try:
             key, value, rex = Prefix.get_prefix(path)
             if key == Prefix.KEY_FIND:
                 return get_pkg_dir(value)+rex
             else:
                 raise Exception("Bad prefixed key '%s'"%key)
         except Exception as ex:
             raise ex
     else:
         return path
def update_moveit_package(args):
    try:
        moveit_config_pkg_path = get_pkg_dir(args.moveit_config_pkg)
    except InvalidROSPkgException:
        raise Exception("Failed to find package: " + args.moveit_config_pkg)

    try:
        srdf_file_name = moveit_config_pkg_path + "/config/" + args.srdf_filename
        srdf = etree.parse(srdf_file_name).getroot()
    except IOError:
        raise Exception("Failed to find SRDF file: " + srdf_file_name)
    except etree.XMLSyntaxError as err:
        raise Exception(
            "Failed to parse xml in file: %s\n%s" % (srdf_file_name, err.msg)
        )

    if args.robot_name_in_srdf != srdf.get("name"):
        raise Exception(
            "Robot name in srdf ('%s') doesn't match expected name ('%s')"
            % (srdf.get("name"), args.robot_name_in_srdf)
        )

    groups = srdf.findall("group")
    if len(groups) < 1:
        raise Exception("No planning groups are defined in the SRDF")

    planning_group = None
    for group in groups:
        if group.get("name").lower() == args.planning_group_name.lower():
            planning_group = group

    if planning_group is None:
        raise Exception(
            "Planning group '%s' not defined in the SRDF. Available groups: \n%s"
            % (
                args.planning_group_name,
                ", ".join([group_name.get("name") for group_name in groups]),
            )
        )

    # Modify kinematics.yaml file
    kin_yaml_file_name = moveit_config_pkg_path + "/config/kinematics.yaml"
    with open(kin_yaml_file_name, "r") as f:
        kin_yaml_data = yaml.safe_load(f)

    kin_yaml_data[args.planning_group_name]["kinematics_solver"] = args.plugin_name
    with open(kin_yaml_file_name, "w") as f:
        yaml.dump(kin_yaml_data, f, default_flow_style=False)

    print("Modified kinematics.yaml at '%s'" % kin_yaml_file_name)
Пример #27
0
def MoveNameToPath(movement_name, current_pos):

    """
    Given a movement name it returns the absolute path to that file.
    In Case it doesn't exist, it will raise an assertion.
    """

    base_package_path = packages.get_pkg_dir(PACKAGE_WHERE_TO_FIND_DANCING_MOVEMENTS)
    aux_database_dir_path = os.path.join(base_package_path, DANCING_DATABASE_DIR)
    aux_area_dir_path = os.path.join(aux_database_dir_path, current_pos)
    absolute_path_mov_name = os.path.join(aux_area_dir_path, movement_name)
    print str(absolute_path_mov_name)
    assert (os.path.exists(absolute_path_mov_name)), "The path to the movement file doesn't exist"
    return absolute_path_mov_name
Пример #28
0
    def test_params(self):
        mock = self._load(os.path.join(self.xml_dir, 'test-params-valid.xml'))

        param_d = {}
        for p in mock.params:
            param_d[p.key] = p.value

        self.assertEquals('pass', param_d['/override'])
        self.assertEquals('bar2', param_d['/somestring1'])
        self.assertEquals('10', param_d['/somestring2'])
        self.assertEquals(1, param_d['/someinteger1'])
        self.assertEquals(2, param_d['/someinteger2'])
        self.assertAlmostEquals(3.14159, param_d['/somefloat1'], 2)
        self.assertAlmostEquals(5.0, param_d['/somefloat2'], 1)
        self.assertEquals("a child namespace parameter 1", param_d['/wg/wgchildparam'], p.value)
        self.assertEquals("a child namespace parameter 2", param_d['/wg2/wg2childparam1'], p.value)
        self.assertEquals("a child namespace parameter 3", param_d['/wg2/wg2childparam2'], p.value)

        import xmlrpclib
        from roslib.packages import get_pkg_dir
        f = open(os.path.join(get_pkg_dir('roslaunch'), 'example.launch'))
        try:
            contents = f.read()
        finally:
            f.close()
        p = [p for p in mock.params if p.key == '/configfile'][0]
        self.assertEquals(contents, p.value, 1)
        p = [p for p in mock.params if p.key == '/binaryfile'][0]
        self.assertEquals(xmlrpclib.Binary(contents), p.value, 1)

        f = open(os.path.join(get_pkg_dir('roslaunch'), 'example.launch'))
        try:
            contents = f.read()
        finally:
            f.close()
        p = [p for p in mock.params if p.key == '/commandoutput'][0]
        self.assertEquals(contents, p.value, 1)
Пример #29
0
def getStandAloneInstance(pkg_name, plugin_class, lng="en"):
    
    plugin_instance = plugin_class(Context(QMainWindow()))
    
    plugin_descriptor = ET.parse("%s/plugin_descriptor.xml"%get_pkg_dir(pkg_name))
    
    plugin_params = PluginProvider.getParameters(plugin_descriptor.getroot(), None)
    
    plugin_instance.tryToCreate(plugin_params)
    
    plugin_instance.tryToResume()
    
    plugin_instance.onTranslate(lng)
    
    return plugin_instance
Пример #30
0
 def _parse(self, launch, dot):
     
     self._launch_directory = launch
     launch_name = launch.split('/')[-1].split('.')[0]
     
     root = None
     try:
         root = ElementTree.parse(self._launch_directory)
     except:
         raise Exception("Invalid launcher path from %s !"%self._launch_directory)
     
     tree = root.getroot()
     
     for elem in tree:
         #<include file="$(find pma_startup)/launch/pma1_driver.launch" />
         if elem.tag == 'include':
             
             include_split = elem.attrib["file"].split(")")
             prefix = include_split[0]
             pkg_name = prefix.split(" ")[-1]
             
             child = include_split[-1].split("/")[-1].split('.')[0]
             
             dot.write('%s [shape=box, style=filled, color=lightgray];\n'%launch_name)
             dot.write('%s [shape=box, style=filled, color=lightgray];\n'%child)
             dot.write("%s -> %s\n"%(launch_name, child))
             
             sub_launch = get_pkg_dir(pkg_name)+include_split[-1]
             
             self._parse(sub_launch, dot)
             
         #<node name="map_server_high_res" pkg="map_server" type="map_server" args="$(arg map_file_high_res)" respawn="false">
         elif elem.tag == "node":
             
             parent = launch_name
             child = Prefix.arg(elem.attrib["name"],tree)
             if parent == child:
                 child += "_node"
             
             dot.write('%s [shape=ellipse, style=filled, color=lightseagreen];\n'%child)
             dot.write("%s -> %s\n"%(parent,child))
def update_moveit_package(args):
  try:
    moveit_config_pkg_path = get_pkg_dir(args.moveit_config_pkg)
  except InvalidROSPkgException:
    raise Exception("Failed to find package: " + args.moveit_config_pkg)

  try:
    srdf_file_name = moveit_config_pkg_path + '/config/' + args.srdf_filename
    srdf = etree.parse(srdf_file_name).getroot()
  except IOError:
    raise Exception("Failed to find SRDF file: " + srdf_file_name)
  except etree.XMLSyntaxError as err:
    raise Exception("Failed to parse xml in file: %s\n%s" % (srdf_file_name, err.msg))

  if args.robot_name_in_srdf != srdf.get('name'):
    raise Exception("Robot name in srdf ('%s') doesn't match expected name ('%s')" %
                    (srdf.get('name'), args.robot_name_in_srdf))

  groups = srdf.findall('group')
  if len(groups) < 1:
    raise Exception("No planning groups are defined in the SRDF")

  planning_group = None
  for group in groups:
    if group.get('name').lower() == args.planning_group_name.lower():
      planning_group = group

  if planning_group is None:
    raise Exception("Planning group '%s' not defined in the SRDF. Available groups: \n%s" % (
        args.planning_group_name, ", ".join([group_name.get('name') for group_name in groups])))

  # Modify kinematics.yaml file
  kin_yaml_file_name = moveit_config_pkg_path + "/config/kinematics.yaml"
  with open(kin_yaml_file_name, 'r') as f:
    kin_yaml_data = yaml.safe_load(f)

  kin_yaml_data[args.planning_group_name]["kinematics_solver"] = args.plugin_name
  with open(kin_yaml_file_name, 'w') as f:
    yaml.dump(kin_yaml_data, f, default_flow_style=False)

  print("Modified kinematics.yaml at '%s'" % kin_yaml_file_name)
Пример #32
0
    def __init__(self, launch, machine):

        self.launch_name = QLabel(launch)
        self.launch_name.setContentsMargins(0, 0, 10, 0)
        self.launch_name.setMinimumHeight(40)

        self.combo_machines = QComboBox()
        self.combo_machines.setMinimumHeight(40)
        self.combo_machines.addItem('cobotgui-dev:127.0.0.1')
        self.combo_machines.addItem('cobot:192.168.0.1')

        rsc = os.path.join(get_pkg_dir('plugin_node_manager'), 'resources')
        icon_launch = QIcon(rsc + '/launch.png')

        self.button_launch = QPushButton()
        self.button_launch.setIcon(icon_launch)
        self.button_launch.setIconSize(QSize(30, 30))
        self.button_launch.setFixedSize(QSize(100, 40))
        self.button_launch.clicked.connect(self._launch_node_slot)

        self.button_launch_widget = widget_creator(self.button_launch)
Пример #33
0
    def arms_out_of_self_colision(self):
        self._print_title("ARMS OUT OF SELF COLISION")

        if self.using_the_robot:
            if self.__check_action(ARMS_ACTION_NAME) == aborted:
                return aborted
        else:
            self._print_warning("Not checking. ROS_MASTER_URI nor COMPUTER_NAME contains %s" % ROBOTS_NAME)
            return aborted

        robot = os.environ.get('PAL_ROBOT')
        ros_master_uri = os.environ.get('ROS_MASTER_URI')
        remotelly_executing = ros_master_uri.rfind('localhost')
        rospy.loginfo('remotelly_executing: %s' % remotelly_executing)  # FIXME: Remove this line after test in the robot
        MOTION_FOLDER_PATH = ''
        plan = False
        checkSafety = False
        if robot == 'rh2c' or robot == 'rh2m' or robot == 'reemh3c' or robot == 'reemh3m' or robot == 'reemh3' or remotelly_executing == -1:
            check_for_door = True  # FIXME: This variable is not being used.
            MOTION_FOLDER_PATH = "/mnt_flash/stacks/reem_alive/alive_engine/config/database/Stopped/interact_1.xml"
        if remotelly_executing == -1:
            MOTION_FOLDER_PATH = packages.get_pkg_dir('pal_smach_utils') + '/config/interact_1.xml'
            plan = True
            checkSafety = False

        motion_goal = MotionManagerGoal()
        motion_goal.plan = plan
        motion_goal.filename = MOTION_FOLDER_PATH
        motion_goal.checkSafety = checkSafety
        motion_goal.repeat = False

        motion_manager_action = SimpleActionState(ARMS_ACTION_NAME, MotionManagerAction, goal=motion_goal)
        userdata_hacked = {}
        status = motion_manager_action.execute(userdata_hacked)

        if status == succeeded:
            rospy.loginfo(self.colors.GREEN_BOLD + "Arms out of self colisin: OK " + self.colors.NATIVE_COLOR)
        else:
            self.ALL_OK = False
            rospy.loginfo(self.colors.RED_BOLD + "Arms out of self colisin: Failed " + self.colors.NATIVE_COLOR)
Пример #34
0
    def __init__(self, gui):

        self._gui = gui

        self._datamodel = QStandardItemModel(0, 3)

        self._gui._table_launch.setModel(self._datamodel)
        self._gui._table_launch.verticalHeader().setVisible(False)

        node_launchers_path = '/'.join(
            [get_pkg_dir('node_launchers'), 'launch'])

        self._launch_list = []

        #         self._gui._table_launch.horizontalHeader().setResizeMode(QHeaderView.Stretch)

        i = 0
        for launch_file in os.listdir(node_launchers_path):
            self._add_launcher(launch_file, '/cobotgui-dev', i)
            i += 1

        self._gui._table_launch.resizeColumnsToContents()
        self._gui._table_launch.resizeRowsToContents()
Пример #35
0
def _manifest_msg_srv_export(ctx, type_):
    exist = []
    for pkg in ctx.pkgs:
        pkg_dir = roslib.packages.get_pkg_dir(pkg)
        d = os.path.join(pkg_dir, type_)
        if os.path.isdir(d):
            files = os.listdir(d)
            if filter(lambda x: x.endswith('.'+type_), files):
                try:
                    m_file = roslib.manifest.manifest_file(pkg, True)
                except InvalidROSPkgException:
                    # ignore wet package from further investigation
                    env = os.environ
                    pkg_path = get_pkg_dir(pkg, True, ros_root=env['ROS_ROOT'])
                    if os.path.exists(os.path.join(pkg_path, PACKAGE_FILE)):
                        continue
                    raise
                m = roslib.manifest.parse_file(m_file)
                cflags = m.get_export('cpp', 'cflags')
                include = '-I${prefix}/%s/cpp'%type_
                if filter(lambda x: include in x, cflags):
                    exist.append(pkg)
    return exist
Пример #36
0
 def __init__(self):
     self.query_batch_num = 3
     pkg_path = rp.get_pkg_dir('prob_grasp_planner')
     self.load_pre_train_pca = True
     self.pre_train_pca_path = pkg_path + '/models/grasp_type_planner/' + \
                           'train_models/pca/pca.model'
     self.load_pre_train_clf = False
     # TODO: if self.load_pre_train_clf is true, the logistic regression training
     # algorithm needs to be changed from the default liblinear to other methods,
     # such as sag or lbfgs, because liblinear doesn't support warm_start being true.
     self.pre_train_clf_path = pkg_path + '/models/grasp_type_planner' + \
                             '/train_models/classifiers/'
     self.load_pre_train_prior = False
     self.pre_train_prior_path = pkg_path + '/models/grasp_type_planner' + \
                       '/train_models/priors_all/'
     self.grasp_data_path = '/mnt/tars_data/grasp_queries/'
     self.save_pca_model = False
     self.gmm_components_num = 2
     self.al_clf_path = pkg_path + '/models/grasp_type_planner' + \
                             '/al_models/classifiers/'
     self.al_prior_path = pkg_path + '/models/grasp_type_planner' + \
                       '/al_models/priors/'
     self.grasp_model_inf = GraspPgmInfer(pgm_grasp_type=True)
Пример #37
0
def _manifest_msg_srv_export(ctx, type_):
    exist = []
    for pkg in ctx.pkgs:
        pkg_dir = roslib.packages.get_pkg_dir(pkg)
        d = os.path.join(pkg_dir, type_)
        if os.path.isdir(d):
            files = os.listdir(d)
            if filter(lambda x: x.endswith('.' + type_), files):
                try:
                    m_file = roslib.manifest.manifest_file(pkg, True)
                except InvalidROSPkgException:
                    # ignore wet package from further investigation
                    env = os.environ
                    pkg_path = get_pkg_dir(pkg, True, ros_root=env['ROS_ROOT'])
                    if os.path.exists(os.path.join(pkg_path, PACKAGE_FILE)):
                        continue
                    raise
                m = roslib.manifest.parse_file(m_file)
                cflags = m.get_export('cpp', 'cflags')
                include = '-I${prefix}/%s/cpp' % type_
                if filter(lambda x: include in x, cflags):
                    exist.append(pkg)
    return exist
Пример #38
0
#! /usr/bin/env python
# -*- encoding: utf-8 -*-


import roslib
roslib.load_manifest('metronome_test')
import rospy
import smach
import os
from roslib import packages
from bpm_analysis import BpmAnalysisSM
from pal_smach_utils.utils.global_common import succeeded, preempted, aborted
from pal_smach_utils.utils.play_sound_sm import PlaySoundAtRateGiven


SOUND_TO_REPEAT_PATH = os.path.join(packages.get_pkg_dir('metronome_test'), 'sounds_lib/146718__fins__button.wav')


class MetronomeDummyState(smach.State):

    def __init__(self):
        smach.State.__init__(self,
                            outcomes=[succeeded, preempted, aborted],
                            output_keys=['sound_to_repeat_path'])

    def execute(self, userdata):

        userdata.sound_to_repeat_path = SOUND_TO_REPEAT_PATH
        print "SOUND TO BE REPEATED = " + str(SOUND_TO_REPEAT_PATH)

        return succeeded
from pal_smach_utils.speech.did_you_say_yes_or_no_sm import HearingConfirmationSM

LISTEN_COMMAND_TIMEOUT = 1.5

SLEEP_TIME_BETWEEN_LISTEN_AND_RECORD_ODOMETRY_ITERATIONS = 2.0

ELEVATOR_DISTANCE_TO_HUMAN = 0.65
follow_grammar_name_index = 0
follow_grammar_names = ["robocup/elevator_out"]  # ['robocup/elevator_in', 'robocup/elevator_out']
follow_grammar_values = ["getout"]  # ['getin', 'getout']
follow_tts_texts = ["I get out of the elevator"]  # ["Ok. I follow you to the elevator.", "I get out of the elevator."]
follow_confirm_messages = ["that you want me to get out of the elevator"]  # ["that you want to get into the elevator", "that you want me to get out of the elevator"]
missunterstood_text = "Sorry, i didn't understand you. Can you repeat please?"

bag = rosbag.Bag(packages.get_pkg_dir('common') + '/config/getin_odometry.bag', 'w')


class RecordOdometry(smach.StateMachine):
    def __init__(self):
        smach.StateMachine.__init__(self, [succeeded, preempted, aborted])

        with self:
            def read_odometry_cb(userdata, message):
                # if the value of the grammar is getout, we already heard a getin command
                # and we can record the odometry till we hear the getout command
                value = follow_grammar_values[follow_grammar_name_index]
                if value == 'getout':
                    try:
                        global bag
                        debugPrint("Writing odometry message to the bag...", 3)
Пример #40
0
# -*- encoding: utf-8 -*-


import roslib
roslib.load_manifest('play_sound_test')
import rospy
import smach
from roslib import packages
from pal_smach_utils.utils.global_common import succeeded, preempted, aborted
from pal_smach_utils.utils.play_sound_sm import SpeakText, PlaySoundOnceState, StopSoundState
from pal_smach_utils.utils.timeout_container import SleepState
import os


TIMEOUT = 10.0
PATH_TO_SEND = os.path.join(packages.get_pkg_dir('dancing_reem'), 'mp3_lib/Daft_Punk_ft_Pharrell_Get_Lucky.wav')
PATH_TO_SEND_WAV = os.path.join(packages.get_pkg_dir('dancing_reem'), 'mp3_lib/Daft_Punk_ft_Pharrell_Get_Lucky.wav')
TEXT_TO_SAY1 = "We are starting the Playing songs test. Let's go!"
TEXT_TO_SAY2 = "We have finished the test. Let's go and bake a pie"


class SoundDummyState(smach.State):

    def __init__(self):
        smach.State.__init__(self,
                             outcomes=[succeeded, preempted, aborted],
                             output_keys=['path', 'text1', 'text2'])

    def execute(self, userdata):

        userdata.path = PATH_TO_SEND
Пример #41
0
#import plot_roc_pr_curve as plot_curve
import matplotlib.pyplot as plt
#import map_classifier as mc

#import os
#os.sys.path.append('./ppca/src/pca')
#import pca
#import ppca as prob_pca
import pickle
from scipy.stats import multivariate_normal
import rospy
from prob_grasp_planner.srv import *
#import proc_grasp_data as pgd
import roslib.packages as rp

pkg_path = rp.get_pkg_dir('prob_grasp_planner')
sys.path.append(pkg_path + '/src/grasp_type_planner')
from grasp_pgm_inference import GraspPgmInfer


class GraspPgmUpdate:
    '''
    Update grasp pgms using active learning grasp queries.
    '''
    def __init__(self):
        self.query_batch_num = 3
        pkg_path = rp.get_pkg_dir('prob_grasp_planner')
        self.load_pre_train_pca = True
        self.pre_train_pca_path = pkg_path + '/models/grasp_type_planner/' + \
                              'train_models/pca/pca.model'
        self.load_pre_train_clf = False
Пример #42
0
            cv.rectangle(output.frame,
                         tl_pt,
                         br_pt,
                         obstacle.output_colour,
                         thickness=2)
    obstacle.publish_msg(position_text, area)

    # Check if the obstacle is selected, show in debug if selected
    if selected_object == field_object_list[obstacle.name]:
        gui.show_debug_window(obstacle_mat.frame, obstacle.name)


if __name__ == '__main__':
    # Load configurations
    configuration = Configuration(configuration_directory=os.path.join(
        rospkg.get_pkg_dir('vision'), 'config'))
    configuration.load()
    colour_space_roi = None

    # Initialize ROS
    rospy.init_node('vision_node')
    rate = rospy.Rate(10)

    # Initialize obstacles/lines and load configurations
    field = Field(configuration=configuration.config['field'])
    lines = Lines(configuration=configuration.config['lines'])
    obstacle = Obstacle(configuration=configuration.config['obstacle'])
    field_object_list = OrderedDict([('field', field), ('lines', lines),
                                     ('obstacle', obstacle)])
    selected_object = field
Пример #43
0
#!/usr/bin/python
# ROS specific imports
import roslib; roslib.load_manifest('floor_nav')
import rospy
import igraph as ig
from math import *
from task_manager_lib.TaskClient import *
from roslib.packages import get_pkg_dir

rospy.init_node('task_client')
server_node = rospy.get_param("~server","/task_server")
default_period = rospy.get_param("~period",0.05)
tc = TaskClient(server_node,default_period)
rospy.loginfo("Mission connected to server: " + server_node)

g = ig.Graph.Read_Picklez(get_pkg_dir('floor_graph')+"/graph.picklez")

tc.WaitForAuto()
try:
    tc.Wait(duration=1.0)
    # Hand-made hamiltonian path
    node_seq = [0, 2, 1, 4, 3, 5, 6, 11, 7, 10, 9, 8]
    for node in node_seq:
        tc.GoTo(goal_x=g.vs[node]["x"],goal_y=g.vs[node]["y"])
    tc.Wait(duration=1.0)

except TaskException, e:
    rospy.logerr("Exception caught: " + str(e))

if not rospy.core.is_shutdown():
    tc.SetManual()
Пример #44
0
            def printOdometryBag(userdata):
                '''
                bag = rosbag.Bag(packages.get_pkg_dir('common') + '/config/getin_odometry.bag')

                odometry_marker_pub = rospy.Publisher("/track_operator/odometry_elevator", Marker)
                i = 0
                for topic, msg, t in bag.read_messages(topics=['/base_odometry/odom']):
                    #rospy.loginfo(str(topic) + '\n\n\n')
                    #rospy.loginfo(str(msg) + '\n\n\n')
                    #rospy.loginfo(str(t) + '\n\n\n')

                    marker = Marker()
                    marker.header.frame_id = "/odom"
                    marker.ns = "odometry_elevator_namespace"
                    marker.id = i
                    marker.type = marker.SPHERE
                    marker.action = marker.ADD
                    marker.pose = msg.pose.pose
                    marker.scale.x = 0.5
                    marker.scale.y = 0.7
                    marker.scale.z = 0.9
                    marker.color.a = 1.0
                    marker.color.r = 0.0
                    marker.color.g = 1.0
                    marker.color.b = 1.0
                    marker.lifetime = rospy.Duration(600.0)
                    marker.header.stamp = rospy.Time.now()

                    rospy.loginfo("\033[00;32m" + str(marker) + "\033[m")

                    odometry_marker_pub.publish(marker)

                    i += 1
                bag.close()
                '''

                bag = rosbag.Bag(packages.get_pkg_dir('common') + '/config/getin_odometry.bag')

                odometry_marker_pub = rospy.Publisher("/track_operator/baselink_to_odometry_elevator", Marker)
                i = 0
                stack = []
                for topic, msg, t in bag.read_messages(topics=['/base_odometry/odom']):
                    #rospy.loginfo(str(topic) + '\n\n\n')
                    #rospy.loginfo(str(msg) + '\n\n\n')
                    #rospy.loginfo(str(t) + '\n\n\n')

                    pose_transformed = transform_pose(msg.pose.pose, '/odom', '/base_link')
                    if (getDebugLevel() >= 3):
                        marker = Marker()
                        marker.header.frame_id = "/base_link"
                        marker.ns = "baselink_to_odometry_elevator_namespace"
                        marker.id = i
                        marker.type = marker.SPHERE
                        marker.action = marker.ADD
                        marker.pose = pose_transformed
                        marker.scale.x = 0.05
                        marker.scale.y = 0.05
                        marker.scale.z = 0.05
                        marker.color.a = 1.0
                        marker.color.r = 0.0
                        marker.color.g = 0.0
                        marker.color.b = 1.0
                        marker.lifetime = rospy.Duration(600.0)
                        marker.header.stamp = rospy.Time.now()
                        odometry_marker_pub.publish(marker)

                        debugPrint("\033[01;33m" + str(marker) + "\033[m", 3)

                    stack.append(pose_transformed)

                    i += 1
                bag.close()

                #TODO SHOULD WORK MOST OF THE TIMES, BUT IF THE BAG IS EMPTY OR WHITH LESS THAN 2 POSES THIS CODE WILL CRASH!!!
                try:
                    initial_pose = stack.pop()
                    final_pose = stack.pop()
                    last_dist = euclidean_distance(initial_pose, final_pose)
                    while (len(stack) > 0 and last_dist < 2.0):
                        new_final_pose = stack.pop()
                        new_dist = euclidean_distance(new_final_pose, initial_pose)
                        if (new_dist > last_dist):
                            last_dist = new_dist
                            final_pose = new_final_pose
                except Exception as ex:
                    debugPrint(str(ex), 0)
                    debugPrint("The bag was empty. Setting a final pose of 2 meters backwards...", 0)
                    initial_pose = Pose()
                    initial_pose.position.x = 0.0
                    initial_pose.position.y = 0.0
                    final_pose = Pose()
                    final_pose.position.x = -2.0
                    final_pose.position.y = 0.0

                marker = Marker()
                marker.header.frame_id = "/base_link"
                marker.ns = "baselink_to_odometry_elevator_namespace"
                marker.id = i
                marker.type = marker.CYLINDER
                marker.action = marker.ADD
                marker.pose = initial_pose
                marker.scale.x = 0.1
                marker.scale.y = 0.1
                marker.scale.z = 2.0
                marker.color.a = 1.0
                marker.color.r = 0.0
                marker.color.g = 1.0
                marker.color.b = 0.0
                marker.lifetime = rospy.Duration(600.0)
                marker.header.stamp = rospy.Time.now()

                debugPrint("    Publishing \033[01;32m(GREEN)\033[mmarker of the INITIAL pose outside the elevator...", 3)
                debugPrint(rospy.loginfo("\033[01;32m" + str(marker) + "\033[m"), 3)

                odometry_marker_pub.publish(marker)

                marker = Marker()
                marker.header.frame_id = "/base_link"
                marker.ns = "baselink_to_odometry_elevator_namespace"
                marker.id = i + 1
                marker.type = marker.CYLINDER
                marker.action = marker.ADD
                marker.pose = final_pose
                marker.scale.x = 0.1
                marker.scale.y = 0.1
                marker.scale.z = 1.0
                marker.color.a = 1.0
                marker.color.r = 1.0
                marker.color.g = 0.0
                marker.color.b = 0.0
                marker.lifetime = rospy.Duration(600.0)
                marker.header.stamp = rospy.Time.now()

                debugPrint("    Publishing \033[01;33m(RED)\033[mmarker of the FINAL pose outside the elevator...", 3)
                debugPrint("    \033[01;33m" + str(marker) + "\033[m", 3)

                odometry_marker_pub.publish(marker)

                userdata.final_pose = final_pose

                return succeeded
#! /usr/bin/env python
# -.- coding: utf-8 -.-
from roslib import packages
from rospy import logwarn
from pal_smach_utils.utils.colors import Colors
import yaml

CONFIG_FILE = packages.get_pkg_dir('cocktail_party') + '/config/cocktail_party.yaml'
CONFIG = yaml.load(open(CONFIG_FILE, 'r'))

ROBOT_GRAMMAR_PATH = "/mnt_flash/etc/interaction/sphinx/model/gram/en_US/"
LOCAL_GRAMMAR_PATH = packages.get_pkg_dir('cocktail_party') + "/config/"

colors = Colors()

class CocktailPartyVariables():
    """CocktailPartyVariables class.

    Use this class to get all variables required by the CocktailPartyStateMachine.
    """
    def __init__(self, print_variables=True):
        """Constructor for CocktailPartyVariables.

        @type print_variables: boolean
        @param print_variables: If is not False, 0 or None, all the variables of this class and its values will be printed.

        """
        self.print_variables = print_variables
        self.TOPICS = []
        self.SERVICES = []
        self.ACTIONS = []
Пример #46
0
#!/usr/bin/env python

import roslib
roslib.load_manifest('grasp_pipeline')
import rospy
from grasp_pipeline.srv import *
from geometry_msgs.msg import Pose, Quaternion
from sensor_msgs.msg import JointState
from std_msgs.msg import Char
from sensor_msgs.msg import JointState
import moveit_msgs.msg
import numpy as np
import roslib.packages as rp
import sys
sys.path.append(rp.get_pkg_dir('hand_services') + '/scripts')
from hand_client import handClient


class ControlAllegroConfig:
    def __init__(self, publish_prefix='/allegro_hand_right'):
        rospy.init_node('control_allegro_config_node')
        self.hand_client = handClient()
        self.dof = 16

    def control_allegro(self, control_type='p'):
        rospy.loginfo('Control allegro to target joint states.')

        target_joint_angles = np.array(
            self.allegro_target_joint_state.position)
        reached = self.hand_client.send_pos_cmd(target_joint_angles)
    def test_resolve_args(self):
        from roslib.substitution_args import resolve_args, SubstitutionException
        from roslib.packages import get_pkg_dir
        roslib_dir = get_pkg_dir('roslib', required=True)

        anon_context = {'foo': 'bar'}
        arg_context = {'fuga': 'hoge', 'car': 'cdr'}
        context = {'anon': anon_context, 'arg': arg_context }
        
        tests = [
            ('$(find roslib)', roslib_dir),
            ('hello$(find roslib)', 'hello'+roslib_dir),
            ('$(find roslib )', roslib_dir),
            ('$$(find roslib )', '$'+roslib_dir),
            ('$( find roslib )', roslib_dir),
            ('$(find  roslib )', roslib_dir),
            ('$(find roslib)$(find roslib)', roslib_dir+roslib_dir),
            ('$(find roslib)/foo/bar.xml', roslib_dir+os.sep+'foo'+os.sep+'bar.xml'),
            (r'$(find roslib)\foo\bar.xml $(find roslib)\bar.xml', roslib_dir+os.sep+'foo'+os.sep+'bar.xml '+roslib_dir+os.sep+'bar.xml'),
            ('$(find roslib)\\foo\\bar.xml more/stuff\\here', roslib_dir+os.sep+'foo'+os.sep+'bar.xml more/stuff\\here'),
            ('$(env ROS_ROOT)', os.environ['ROS_ROOT']),
            ('$(env ROS_ROOT)', os.environ['ROS_ROOT']),
            ('$(env ROS_ROOT )', os.environ['ROS_ROOT']),
            ('$(optenv ROS_ROOT)', os.environ['ROS_ROOT']),
            ('$(optenv ROS_ROOT)$(optenv ROS_ROOT)', os.environ['ROS_ROOT']+os.environ['ROS_ROOT']),
            ('$(optenv ROS_ROOT alternate text)', os.environ['ROS_ROOT']),
            ('$(optenv NOT_ROS_ROOT)', ''),
            ('$(optenv NOT_ROS_ROOT)more stuff', 'more stuff'),
            ('$(optenv NOT_ROS_ROOT alternate)', 'alternate'),
            ('$(optenv NOT_ROS_ROOT alternate text)', 'alternate text'),

            # #1776
            ('$(anon foo)', 'bar'),
            ('$(anon foo)/baz', 'bar/baz'),
            ('$(anon foo)/baz/$(anon foo)', 'bar/baz/bar'),

            # arg
            ('$(arg fuga)', 'hoge'),
            ('$(arg fuga)$(arg fuga)', 'hogehoge'),
            ('$(arg car)$(arg fuga)', 'cdrhoge'),
            ('$(arg fuga)hoge', 'hogehoge'),
            ]
        for arg, val in tests:
            self.assertEquals(val, resolve_args(arg, context=context))

        # more #1776
        r = resolve_args('$(anon foo)/bar')
        self.assert_('/bar' in r)
        self.failIf('$(anon foo)' in r)        
        
            
        # test against strings that should not match
        noop_tests = [
            '$(find roslib', '$find roslib', '', ' ', 'noop', 'find roslib', 'env ROS_ROOT', '$$', ')', '(', '()',
            None, 
            ]
        for t in noop_tests:
            self.assertEquals(t, resolve_args(t))
        failures = [
            '$((find roslib))',  '$(find $roslib)',
            '$(find)', '$(find roslib roslib)', '$(export roslib)',
            '$(env)', '$(env ROS_ROOT alternate)',
            '$(env NOT_SET)',
            '$(optenv)',
            '$(anon)',
            '$(anon foo bar)',            
            ]
        for f in failures:
            try:
                resolve_args(f)
                self.fail("resolve_args(%s) should have failed"%f)
            except SubstitutionException: pass
Пример #48
0
import roslib

roslib.load_manifest("execute_movement_xml_test")
import rospy
import smach
import smach_ros
from roslib import packages
from pal_smach_utils.utils.global_common import succeeded, preempted, aborted
from execute_movement import ExecuteMovement
import os
from pal_smach_utils.utils.check_dependences import CheckDependencesState
from pal_smach_utils.utils.time_controlling_states import GetCurrentROSTimeState
from write_old_movement import WriteOldMovement

DUMB_MOVEMENT_XML_NAME = os.path.join(
    packages.get_pkg_dir("dancing_reem"),
    "dancing_movements_database/testing_movement_files/transition_from_middle_to_sides_without_parameter.xml",
)
DUMB_MOVEMENT_XML_NAME2 = os.path.join(
    packages.get_pkg_dir("dancing_reem"),
    "dancing_movements_database/testing_movement_files/waiting_to_send_movement.xml",
)


# DUMB_MOVEMENT_XML_NAME = '/mnt_flash/robocup2013/reem_at_iri/dancing_reem/dancing_movements_database/testing_movement_files/transition_from_middle_to_sides_without_parameter.xml'
# DUMB_MOVEMENT_XML_NAME2 = '/mnt_flash/robocup2013/reem_at_iri/dancing_reem/dancing_movements_database/testing_movement_files/transition_from_sides_to_middle_without_parameter.xml'

"""
robot = os.environ.get('PAL_ROBOT')
ros_master_uri = os.environ.get('ROS_MASTER_URI')
remotelly_executing = ros_master_uri.rfind('localhost')
Пример #49
0
# (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
# ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
# (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
# SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#
# Please send comments, questions, or patches to [email protected]
#

import roslib; roslib.load_manifest('applanix_generated_msgs')
from roslib.packages import get_pkg_dir
from mapping import groups, msgs
from os import path
from sys import stdout

pkg_dir = get_pkg_dir('applanix_generated_msgs')
output = []

all_msgs_filename = path.join(pkg_dir, "msg", "AllMsgs.msg")
with open(all_msgs_filename, "w") as all_msgs_f:
  all_msgs_f.write("time last_changed\n")
  all_msgs_f.write("time last_sent\n")
  for msg_num in msgs.keys():
    msg_tuple = msgs[msg_num]
    if msg_tuple:
      name, msg_cls = msg_tuple
      if msg_cls.in_all_msgs: 
        all_msgs_f.write("applanix_msgs/%s %s\n" % (msg_cls.__name__, name))

      if name != "ack":
        msg_srv_filename = path.join(pkg_dir, "srv", "%s.srv" % msg_cls.__name__)
Пример #50
0
# ROS specific imports
import roslib
roslib.load_manifest('floor_nav')
import rospy
import igraph as ig
from math import *
from task_manager_lib.TaskClient import *
from roslib.packages import get_pkg_dir

rospy.init_node('task_client')
server_node = rospy.get_param("~server", "/task_server")
default_period = rospy.get_param("~period", 0.05)
tc = TaskClient(server_node, default_period)
rospy.loginfo("Mission connected to server: " + server_node)

g = ig.Graph.Read_Picklez(get_pkg_dir('floor_graph') + "/graph.picklez")

tc.WaitForAuto()
try:
    while True:
        tc.Wait(duration=1.0)
        # Hand-made hamiltonian path
        node_seq = [2, 1, 4, 3, 5, 6, 0, 9, 8, 10, 7, 11]
        for node in node_seq:
            tc.GoTo(goal_x=g.vs[node]["x"],
                    goal_y=g.vs[node]["y"],
                    max_velocity=0.5,
                    k_v=1.0,
                    max_angular_velocity=1.0)
        tc.Wait(duration=1.0)
        node_seq.reverse()
Пример #51
0
    def test_resolve_args(self):
        from roslib.substitution_args import resolve_args, SubstitutionException
        from roslib.packages import get_pkg_dir
        rospy_dir = get_pkg_dir('rospy', required=True)

        anon_context = {'foo': 'bar'}
        arg_context = {'fuga': 'hoge', 'car': 'cdr'}
        context = {'anon': anon_context, 'arg': arg_context }
        
        tests = [
            ('$(find rospy)', rospy_dir),
            ('hello$(find rospy)', 'hello'+rospy_dir),
            ('$(find rospy )', rospy_dir),
            ('$$(find rospy )', '$'+rospy_dir),
            ('$( find rospy )', rospy_dir),
            ('$(find  rospy )', rospy_dir),
            ('$(find rospy)$(find rospy)', rospy_dir+rospy_dir),
            ('$(find rospy)/foo/bar.xml', rospy_dir+os.sep+'foo'+os.sep+'bar.xml'),
            (r'$(find rospy)\foo\bar.xml $(find rospy)\bar.xml', rospy_dir+os.sep+'foo'+os.sep+'bar.xml '+rospy_dir+os.sep+'bar.xml'),
            ('$(find rospy)\\foo\\bar.xml more/stuff\\here', rospy_dir+os.sep+'foo'+os.sep+'bar.xml more/stuff\\here'),
            ('$(env ROS_ROOT)', os.environ['ROS_ROOT']),
            ('$(env ROS_ROOT)', os.environ['ROS_ROOT']),
            ('$(env ROS_ROOT )', os.environ['ROS_ROOT']),
            ('$(optenv ROS_ROOT)', os.environ['ROS_ROOT']),
            ('$(optenv ROS_ROOT)$(optenv ROS_ROOT)', os.environ['ROS_ROOT']+os.environ['ROS_ROOT']),
            ('$(optenv ROS_ROOT alternate text)', os.environ['ROS_ROOT']),
            ('$(optenv NOT_ROS_ROOT)', ''),
            ('$(optenv NOT_ROS_ROOT)more stuff', 'more stuff'),
            ('$(optenv NOT_ROS_ROOT alternate)', 'alternate'),
            ('$(optenv NOT_ROS_ROOT alternate text)', 'alternate text'),

            # #1776
            ('$(anon foo)', 'bar'),
            ('$(anon foo)/baz', 'bar/baz'),
            ('$(anon foo)/baz/$(anon foo)', 'bar/baz/bar'),

            # arg
            ('$(arg fuga)', 'hoge'),
            ('$(arg fuga)$(arg fuga)', 'hogehoge'),
            ('$(arg car)$(arg fuga)', 'cdrhoge'),
            ('$(arg fuga)hoge', 'hogehoge'),
            ]
        for arg, val in tests:
            self.assertEquals(val, resolve_args(arg, context=context))

        # more #1776
        r = resolve_args('$(anon foo)/bar')
        self.assert_('/bar' in r)
        self.failIf('$(anon foo)' in r)        
        
            
        # test against strings that should not match
        noop_tests = [
            '$(find rospy', '$find rospy', '', ' ', 'noop', 'find rospy', 'env ROS_ROOT', '$$', ')', '(', '()',
            None, 
            ]
        for t in noop_tests:
            self.assertEquals(t, resolve_args(t))
        failures = [
            '$((find rospy))',  '$(find $rospy)',
            '$(find)', '$(find rospy roslib)', '$(export rospy)',
            '$(env)', '$(env ROS_ROOT alternate)',
            '$(env NOT_SET)',
            '$(optenv)',
            '$(anon)',
            '$(anon foo bar)',            
            ]
        for f in failures:
            try:
                resolve_args(f)
                self.fail("resolve_args(%s) should have failed"%f)
            except SubstitutionException: pass
Пример #52
0
########################################
# Module(s) declaration
########################################

import rospy
import os
from roslib.packages import get_pkg_dir
from python_qt_binding.QtGui import *
from python_qt_binding.QtCore import *
from python_qt_binding import loadUi

########################################
# Constante(s) and Variable(s) declaration
########################################

DIR_SSMPLUGIN_RESOURCES = os.path.join(get_pkg_dir('ssm_plugin'), 'resources')
DIR_SSMPLUGIN_VALUES = DIR_SSMPLUGIN_RESOURCES + '/values'
DIR_SSMPLUGIN_IMAGES = DIR_SSMPLUGIN_RESOURCES + '/images'
DIR_SSMPLUGIN_LAYOUTS = DIR_SSMPLUGIN_RESOURCES + '/layouts'

########################################
# Class(ies) declaration
########################################


class SsmPluginValues():
    def __init__(self):
        class SsmPluginStrings():
            def __init__(self):
                self.uuid = self.__class__.__name__
Пример #53
0
from pal_smach_utils.navigation.track_operator import TrackOperator

from pal_smach_utils.navigation.track_operator import LearnPerson

from pal_navigation_msgs.srv import DisableEmergency, DisableEmergencyRequest
from pal_control_msgs.msg import MotionManagerGoal, MotionManagerAction

from smach_ros import ServiceState, SimpleActionState

from std_srvs.srv import Empty

import os
from roslib import packages
ros_master_uri = os.environ.get('ROS_MASTER_URI')
remotelly_executing = ros_master_uri.rfind('localhost')
MOTION_FOLDER_PATH = packages.get_pkg_dir('pal_smach_utils') + '/config/interact_1.xml'
if remotelly_executing == -1:
    MOTION_FOLDER_PATH = "/mnt_flash/robocup2013/reem_at_iri/state_machines/common/config/interact_1.xml"

inside_robot = False
robot = os.environ.get('PAL_ROBOT')
if robot == 'rh2c' or robot == 'rh2m' or robot == 'reemh3c' or robot == 'reemh3m' or robot == 'reemh3':
    inside_robot = True


from pr2_controllers_msgs.msg import PointHeadGoal, PointHeadAction

from actionlib import SimpleActionClient

from smach import CBState
Пример #54
0
#!/usr/bin/env python

from roslib.packages import get_pkg_dir
import sys

sys.path = [get_pkg_dir('ardrone2_mudd')+"/nodes"]+sys.path

import ardrone2
from ardrone2 import rospy
from std_msgs.msg import String

class myArdrone(ardrone2.Ardrone):
    def __init__(self):
      ardrone2.Ardrone.__init__(self)
      self.state           = "keyboard"
      self.boxX            = 0
      self.boxHeight       = 0
      self.tarHeight       = .23
      self.tarX            = .46
      self.toleranceX      = 40/320.0
      self.toleranceHeight = .03
      self.toleranceArea   = .001
      self.noBox           = True
      self.spinPower       = .4
      self.forwardPower    = .25

    def loop(self):
      while not rospy.is_shutdown():
        char = self.getKeyPress(1000)
        if char == 'n':
          self.state = "start"
#! /usr/bin/env python

import roslib
roslib.load_manifest('wait_to_send_movement_test')
import os
import rospy
import smach
import smach_ros
from roslib import packages
from pal_smach_utils.utils.global_common import succeeded, preempted, aborted
from pal_smach_utils.utils.time_out import TimeOut
from wait_to_send_next_movement import WaitToSendNextMovement


#DUMB_MOVEMENT_XML_NAME = os.path.join(packages.get_pkg_dir('dancing_reem'), 'dancing_movements_database/testing_movement_files/transition_from_middle_to_sides_without_parameter.xml')
DUMB_MOVEMENT_XML_NAME = os.path.join(packages.get_pkg_dir('dancing_reem'), 'dancing_movements_database/testing_movement_files/waiting_to_send_movement.xml')

#DUMB_MOVEMENT_XML_NAME = '/mnt_flash/robocup2013/reem_at_iri/dancing_reem/dancing_movements_database/testing_movement_files/transition_from_middle_to_sides_without_parameter.xml'

HARMONICS = 1.0
# Send a movement every 3 seconds.
BPM_TEST = 20

MOVING_MOCK_TIME = 2.0


class InitVars(smach.State):

    def __init__(self):
        smach.State.__init__(self,
                             outcomes=[succeeded, preempted, aborted],
Пример #56
0
 def setUp(self):
     from roslib.packages import get_pkg_dir
     self.xml_dir = os.path.join(get_pkg_dir('test_roslaunch'), 'test', 'xml')