Пример #1
0
  protected static String getPlanningGroups(ArrayList<PlanningGroup> groups, String robotName) {

    String i1 = indent;
    String i2 = i1 + indent;

    StringBuffer s = new StringBuffer();

    if (groups != null && groups.size() > 0) {

      s.append("\n\n");
      s.append(i1 + "<!-- =========================================== -->\n");
      s.append(i1 + "<!-- | Planning Groups                         | -->\n");
      s.append(i1 + "<!-- =========================================== -->\n");

      for (PlanningGroup pg : groups) {

        s.append("\n\n");
        s.append(i1 + "<!-- &robot;" + pg.getName() + " -->\n\n");
        s.append(i1 + "<owl:NamedIndividual rdf:about=\"&robot;" + pg.getName() + "\">\n");
        s.append(i2 + "<rdf:type rdf:resource=\"&srdl2-comp;ComponentComposition\"/>\n");
        s.append(i2 + "<rdf:type rdf:resource=\"&srdl2-comp;ArmComponent\"/>\n");
        s.append(
            i2
                + "<srdl2-comp:baseLinkOfComposition rdf:resource=\"&robot;"
                + pg.getBaseLink()
                + "\"/>\n");
        s.append(
            i2
                + "<srdl2-comp:endLinkOfComposition rdf:resource=\"&robot;"
                + pg.getTipLink()
                + "\"/>\n");
        s.append(i1 + "</owl:NamedIndividual>\n");
      }
    }

    return s.toString();
  }
Пример #2
0
  public ArrayList<PlanningGroup> loadPlanningGroups(String filename) {

    ArrayList<PlanningGroup> groups = new ArrayList<PlanningGroup>();

    System.out.println("[INFO]\tLoading planning groups from '" + filename + "' ...");

    try {
      InputStream input = new FileInputStream(new File(filename));

      Yaml yaml = new Yaml(new SafeConstructor());
      Map<?, ?> data = (Map<?, ?>) yaml.load(input);
      Set<?> set = data.keySet();
      String[] keys = set.toArray(new String[set.size()]);
      for (String key : keys) {
        if (key.equalsIgnoreCase("groups")) {
          ArrayList<?> values = (ArrayList<?>) data.get(key);
          Map<?, ?>[] groupMaps = values.toArray(new Map<?, ?>[values.size()]);
          PlanningGroup pg = null;
          for (Map<?, ?> groupMap : groupMaps) {
            Set<?> groupKeySet = groupMap.keySet();
            String[] groupKeys = groupKeySet.toArray(new String[groupKeySet.size()]);
            for (String groupKey : groupKeys) {
              if (groupKey.equalsIgnoreCase("name")) {

                if (pg == null) {
                  pg = new PlanningGroup();
                }
                String groupName = (String) groupMap.get(groupKey);
                if (pg.isNameSet()) {
                  throw new Exception(
                      "[ERROR] name is set more than one time ("
                          + pg.getName()
                          + ", "
                          + groupName
                          + ")");
                } else {
                  pg.setName(groupName);
                }

              } else if (groupKey.equalsIgnoreCase("base_link")) {

                if (pg == null) {
                  pg = new PlanningGroup();
                }
                String baseLink = (String) groupMap.get(groupKey);
                if (pg.isBaseLinkSet()) {
                  throw new Exception(
                      "[ERROR] base_link is set more than one time ("
                          + pg.getBaseLink()
                          + ", "
                          + baseLink
                          + ")");
                } else {
                  pg.setBaseLink(baseLink);
                }

              } else if (groupKey.equalsIgnoreCase("tip_link")) {

                if (pg == null) {
                  pg = new PlanningGroup();
                }
                String tipLink = (String) groupMap.get(groupKey);
                if (pg.isTipLinkSet()) {
                  throw new Exception(
                      "[ERROR] tip_link is set more than one time ("
                          + pg.getTipLink()
                          + ", "
                          + tipLink
                          + ")");
                } else {
                  pg.setTipLink(tipLink);
                }
              }
            }

            if (pg != null && pg.isAllSet()) {
              groups.add(pg);
              pg = null;
            } else {
              throw new Exception("[ERROR] planning group misses parameter");
            }
          }
          break;
        }
      }

    } catch (FileNotFoundException fnfe) {
      System.out.println(
          "[WARNING] Could not find file "
              + filename
              + "! "
              + "No planning groups will be created in SRDL.");
      fnfe.printStackTrace();
      groups.clear();
    } catch (Exception e) {
      System.out.println(
          "[WARNING] Unexpected file format ("
              + filename
              + ")! "
              + "No planning groups will be created in SRDL.");
      e.printStackTrace();
      groups.clear();
    }

    return groups;
  }