Newer
Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
import java.io.*;
import java.util.*;
import org.jdom2.*;
import org.jdom2.filter.Filters;
import org.jdom2.input.SAXBuilder;
import org.jdom2.output.XMLOutputter;
import org.jdom2.xpath.*;
/**
* Takes a featuremodel.xml file generated using FeatureModelConfigurator and a given
* ROS/ Gazebo .gazebo.xacro file.
* The .gazebo.xacro file is modified to support the features specified in the
* featuremodel.xml file.
*/
public class Adapter {
/**
* Reads an .xml file from file path and returns a JDOM document.
*
* @param src The .xml file path
* @return JDOM document
* @throws JDOMException
* @throws IOException
*/
public static Document readXml(String src) throws JDOMException, IOException {
File file = new File(src);
SAXBuilder saxBuilder = new SAXBuilder();
Document doc = saxBuilder.build(file);
return doc;
}
/**
* Saves the given Document as XML to the given path.
*
* @param oDoc The JDOM document
* @param sPath The output path
* @throws IOException
*/
public static void saveAsXml(Document oDoc, String sPath) throws IOException {
FileOutputStream dest = new FileOutputStream(sPath);
XMLOutputter xmlOutput = new XMLOutputter();
xmlOutput.output(oDoc, dest);
}
/**
* Executes given XPath expression for elements with JDOM Document as source.
*
* @param doc The JDOM Document
* @param xPath XPath expression
* @return List with all elements found or empty list if no match found
*/
public static List<Element> xQueryElements(Document doc, String xPath) {
XPathFactory xFactory = XPathFactory.instance();
XPathExpression<Element> expr = xFactory.compile(xPath, Filters.element());
List<Element> result = expr.evaluate(doc);
return result;
}
/**
* Executes given XPath expression for elements with Element as source.
*
* @param elem The Element
* @param xPath XPath expression
* @return List with all elements found or empty list if no match found
*/
public static List<Element> xQueryElements(Element elem, String xPath) {
XPathFactory xFactory = XPathFactory.instance();
XPathExpression<Element> expr = xFactory.compile(xPath, Filters.element());
List<Element> result = expr.evaluate(elem);
return result;
}
/**
* Executes given XPath expression for attributes.
*
* @param doc The JDOM Document
* @param xPath XPath expression
* @return List with all attributes found or empty list if no match found
*/
public static List<Attribute> xQueryAttributes(Document doc, String xPath) {
XPathFactory xFactory = XPathFactory.instance();
XPathExpression<Attribute> expr = xFactory.compile(xPath, Filters.attribute());
List<Attribute> result = expr.evaluate(doc);
return result;
}
/**
* Searches a document for the first occurrence of an xml tag.
*
* @param doc The JDOM Document
* @param elementName Name of the xml tag
* @return First element found or NULL if no match found
*/
public static Element searchFirstElement(Document doc, String elementName) {
//XPath index starts with 1; [] has a higher precedence than //
List<Element> query= xQueryElements(doc, "(//"+elementName+")[1]");
if(query.isEmpty()) return null;
return query.get(0);
}
/**
* Searches a document for all occurrences of an xml tag.
*
* @param doc The JDOM Document
* @param elementName Name of the xml tag
* @return List with all elements found or empty list if no match found
*/
public static List<Element> searchAllElements(Document doc, String elementName) {
return xQueryElements(doc, "//"+elementName);
}
/**
* Checks whether two elements have an equal xml tag.
* (Name of the xml tag and all attributes including their order must be equal)
*
* @param element1 Element1
* @param element2 Element2
* @return true if of equal tag or false if not
*/
public static boolean equalTag(Element element1, Element element2) {
List<Attribute> attributeList1 = element1.getAttributes();
List<Attribute> attributeList2 = element2.getAttributes();
if(!element1.getName().equals(element2.getName())) return false;
if(attributeList1.size()!=attributeList2.size()) return false;
for(int i=0; i<attributeList1.size(); i++) {
if(!attributeList1.get(i).getName().equals(attributeList2.get(i).getName())) return false;
if(!attributeList1.get(i).getValue().equals(attributeList2.get(i).getValue())) return false;
}
return true;
}
/**
* Recursive replacement of all text values in destElements with their corresponding
* new ones from srcElements.
* (srcElements and destElements must be matching tags!)
*
* TODO maybe add tags that are in srcElements but not yet in destElements
*
* @param srcElements Element list with new values
* @param destElements Element list with values to be replaced
* @return true if reached deepest Element or false if not
*/
public static boolean replace(List<Element> srcElements, List<Element> destElements) {
if(srcElements.isEmpty()&&destElements.isEmpty()) return true;
if(srcElements.isEmpty()^destElements.isEmpty()) {
System.err.println("Error during value replacement: Sensor xml trees have mismatching deph (maybe tags with no value)");
System.exit(-1);
}
for(Element src : srcElements) {
for(Element dest : destElements) {
if(equalTag(src, dest)) {
boolean deadEnd = replace(src.getChildren(), dest.getChildren());
if(deadEnd) dest.setText(src.getText());
break;
}
}
}
* Obtains the path attribute for the input/ output files as specified in adapterconfig.xml.
*
* @param doc The JDOM Document (adapterconfig.xml)
* @param str {input or output}/{entity}
* @return path as specified in adapterconfig.xml
*/
private static String getConfigPath(Document doc, String str) {
List<Attribute> pathList = xQueryAttributes(doc, "//"+str+"/@path");
if(pathList.size()!=1) {
System.err.println("Error while reading input/ output path from adapterconfig.xml");
System.exit(-1);
}
return pathList.get(0).getValue();
}
/**
* Obtains the name attribute for the input/ output files as specified in adapterconfig.xml.
*
* @param doc The JDOM Document (adapterconfig.xml)
* @param str {input or output}/{entity}
* @return name as specified in adapterconfig.xml
*/
private static String getConfigName(Document doc, String str) {
List<Attribute> nameList = xQueryAttributes(doc, "//"+str+"/@name");
if(nameList.size()!=1) {
System.err.println("Error while reading input/ output name from adapterconfig.xml");
System.exit(-1);
}
return nameList.get(0).getValue();
}
/**
* @param args
*/
public static void main(String[] args) {
try {
//TODO##### Input ###############################
Document adapterConfig = readXml("./adapterconfig.xml");
String in_fmPath = getConfigPath(adapterConfig, "input/featuremodel");
String in_scPath = getConfigPath(adapterConfig, "input/sensorconfig");
String in_gmPath = getConfigPath(adapterConfig, "input/gazebomodel");
String out_gmPath = getConfigPath(adapterConfig, "output/gazebomodel");
//String in_gmName =
String out_gmName = getConfigName(adapterConfig, "output/gazebomodel");
Document featureModel = readXml(in_fmPath);
Document sensorConfig = readXml(in_scPath);
List<Attribute> botList = xQueryAttributes(featureModel, "//module[@types='TurtleBot']/@name");
String botName = "";
String fileName = "";
switch(botList.size()) {
case 0:
System.exit(-1);
break;
case 1:
botName = botList.get(0).getValue();
break;
default:
//TODO get all .gazebo.xacro file names from /turtlebot3_description/urdf/ directory; match botName; select file
switch(botName) {
case "Burger":
fileName = "turtlebot3_burger.gazebo.xacro";
break;
case "Waffle":
fileName = "turtlebot3_waffle.gazebo.xacro";
break;
case "WafflePi":
fileName = "turtlebot3_waffle_pi.gazebo.xacro";
break;
default:
System.err.println("Cannot handle \'"+botName+"\': No input file known.");
//###############################################
//TODO##### Operate on .gazebo.xacro ############
List<Element> cSensorList = xQueryElements(sensorConfig, "//sensor");
List<Element> gSensorList = xQueryElements(gazeboModel, "//sensor");
if(cSensorList.isEmpty()) {
System.out.println("No new sensor values in sensorconfig.xml");
System.exit(-1);
}
if(gSensorList.isEmpty()) {
System.out.println("No sensors found in "+fileName);
System.exit(-1);
}
for(Element cSensor : cSensorList) {
for(Element gSensor : gSensorList) {
if(equalTag(cSensor, gSensor)) {
replace(cSensor.getChildren(),gSensor.getChildren());
if(!match) System.out.println("No matching sensors could be found in sensorconfig.xml and "+fileName);
//###############################################
//######### Output result #######################
saveAsXml(gazeboModel, out_gmPath+out_gmName);
//###############################################
} catch (JDOMException | IOException e) {
e.printStackTrace();
}
}
}
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
//Example dump
//hard-coded and only for demo purposes
//List<Element> cSensorList = xQueryElements(sensorConfig, "//sensor[@type='ray' and @name='lds_lfcd_sensor']");
//Element cSensor = cSensorList.get(0);
//
//List<Element> gSensorList = xQueryElements(gazeboModel, "//sensor[@type='ray' and @name='lds_lfcd_sensor']");
//Element gSensor = gSensorList.get(0);
//
//String new_min = cSensor.getChild("range").getChild("min").getText();
//String new_max = cSensor.getChild("range").getChild("max").getText();
//
//gSensor.getChild("ray").getChild("range").getChild("min").setText(new_min);
//gSensor.getChild("ray").getChild("range").getChild("max").setText(new_max);
//XPathExamples
//XPathFactory xFactory = XPathFactory.instance();
//XPathExpression<Element> xExpr = xFactory.compile("robot/gazebo/sensor/ray/range/min", Filters.element());
//Element test = xExpr.evaluateFirst(gazeboModel);
//List<Element> test2 = searchAllElements(gazeboModel, "min");
//List<Element> test3 = searchAllElements(gazeboModel, "max");
//Element test4 = searchFirstElement(gazeboModel, "plugin");
//List<Attribute> test5 = xQueryAttributes(gazeboModel, "(//plugin)[1]/@*");
//
//if(test != null) xmlOutput.output(test, System.out);
//System.out.println("\n");
//xmlOutput.output(test2, System.out);
//System.out.println("\n");
//xmlOutput.output(test3, System.out);
//System.out.println("\n");
//if(test4 != null) xmlOutput.output(test4, System.out);
//System.out.println("\n");
//for(Attribute a : test5) System.out.println(a.getName()+"="+a.getValue());