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();
}
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;
}
