2 // Copyright (C) 2009 - 2010 Mathias Fröhlich <Mathias.Froehlich@web.de>
4 // This program is free software; you can redistribute it and/or
5 // modify it under the terms of the GNU General Public License as
6 // published by the Free Software Foundation; either version 2 of the
7 // License, or (at your option) any later version.
9 // This program is distributed in the hope that it will be useful, but
10 // WITHOUT ANY WARRANTY; without even the implied warranty of
11 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
12 // General Public License for more details.
14 // You should have received a copy of the GNU General Public License
15 // along with this program; if not, write to the Free Software
16 // Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
25 #include <simgear/compiler.h>
33 #include <simgear/debug/logstream.hxx>
34 #include <simgear/math/SGMath.hxx>
35 #include <simgear/misc/sg_path.hxx>
36 #include <simgear/props/props.hxx>
37 #include <simgear/structure/exception.hxx>
38 #include <simgear/xml/easyxml.hxx>
40 #include <simgear/hla/HLAFederate.hxx>
41 #include <simgear/hla/HLAArrayDataElement.hxx>
42 #include <simgear/hla/HLADataElement.hxx>
43 #include <simgear/hla/HLADataType.hxx>
44 #include <simgear/hla/HLALocation.hxx>
45 #include <simgear/hla/HLAObjectClass.hxx>
46 #include <simgear/hla/HLAObjectInstance.hxx>
47 #include <simgear/hla/HLAPropertyDataElement.hxx>
48 #include <simgear/hla/HLAVariantDataElement.hxx>
50 #include <AIModel/AIMultiplayer.hxx>
51 #include <AIModel/AIManager.hxx>
53 #include <FDM/flightProperties.hxx>
55 #include <Main/fg_props.hxx>
56 #include <Main/globals.hxx>
59 // FIXME make classes here private ...
61 namespace sg = simgear;
69 class FGHLA::XMLConfigReader : public XMLVisitor {
75 const std::string& getFederateObjectModel() const
76 { return _federateObjectModel; }
78 HLAVersion getRTIVersion() const
79 { return _rtiVersion; }
80 const std::list<std::string>& getRTIArguments() const
81 { return _rtiArguments; }
86 std::string _inProperty;
87 std::string _outProperty;
89 typedef std::list<DataElement> DataElementList;
92 DataElementList _dataElementList;
93 std::list<std::pair<std::string, std::string> > _modelMap;
95 struct SimTimeConfig {
97 DataElementList _dataElementList;
99 struct PositionConfig {
101 DataElementList _dataElementList;
103 struct MPPropertiesConfig {
106 struct ObjectClassConfig {
109 ModelConfig _modelConfig;
110 SimTimeConfig _simTimeConfig;
111 PositionConfig _positionConfig;
112 MPPropertiesConfig _mpPropertiesConfig;
113 DataElementList _dataElementList;
115 typedef std::list<ObjectClassConfig> ObjectClassConfigList;
117 const ObjectClassConfigList& getObjectClassConfigList() const
118 { return _objectClassConfigList; }
121 virtual void startXML()
123 _modeStack.push(TopLevelMode);
125 virtual void endXML()
129 virtual void startElement(const char* name, const XMLAttributes& atts)
131 if (strcmp(name, "hlaConfiguration") == 0) {
132 if (!isModeStackTop(TopLevelMode))
133 throw sg_exception("hlaConfiguration tag not at top level");
135 _modeStack.push(HLAConfigurationMode);
138 else if (strcmp(name, "rti") == 0) {
139 if (!isModeStackTop(HLAConfigurationMode))
140 throw sg_exception("rti tag not at top level");
142 std::string rtiVersion = getAttribute("version", atts);
143 if (rtiVersion == "RTI13")
145 else if (rtiVersion == "RTI1516")
146 _rtiVersion = RTI1516;
147 else if (rtiVersion == "RTI1516E")
148 _rtiVersion = RTI1516E;
150 throw sg_exception("invalid version attribute in rti tag");
152 _modeStack.push(RTIMode);
154 else if (strcmp(name, "argument") == 0) {
155 if (!isModeStackTop(RTIMode))
156 throw sg_exception("argument tag not inside an rti tag");
158 _modeStack.push(RTIArgumentMode);
159 _rtiArguments.push_back(std::string());
162 else if (strcmp(name, "objects") == 0) {
163 if (!isModeStackTop(HLAConfigurationMode))
164 throw sg_exception("objects tag not at top level");
166 _modeStack.push(ObjectsMode);
168 else if (strcmp(name, "objectClass") == 0) {
169 // Currently this is flat and only allows one class to be configured
170 if (!isModeStackTop(ObjectsMode))
171 throw sg_exception("objects tag not inside an objects tag");
172 if (!_objectClassConfigList.empty())
173 throw sg_exception("currently only one objectClass is allowed");
174 // if (!isModeStackTop(ObjectsMode) && !isModeStackTop(ObjectClassMode))
175 // throw sg_exception("objects tag not inside an objects or objectClass tag");
177 ObjectClassConfig objectClassConfig;
178 objectClassConfig._type = getAttribute("type", atts);
179 objectClassConfig._name = getAttribute("name", atts);
180 _objectClassConfigList.push_back(objectClassConfig);
182 _modeStack.push(ObjectClassMode);
184 else if (strcmp(name, "model") == 0) {
185 if (!isModeStackTop(ObjectClassMode))
186 throw sg_exception("position tag not inside an objectClass tag");
188 _objectClassConfigList.back()._modelConfig._type = getAttribute("type", atts);
190 _modeStack.push(ModelMode);
192 else if (strcmp(name, "key") == 0) {
193 if (!isModeStackTop(ModelMode))
194 throw sg_exception("key tag not inside an model tag");
196 std::pair<std::string,std::string> p;
197 p.first = getAttribute("external", atts);
198 p.second = getAttribute("modelPath", atts);
199 _objectClassConfigList.back()._modelConfig._modelMap.push_back(p);
201 _modeStack.push(KeyMode);
203 else if (strcmp(name, "simTime") == 0) {
204 if (!isModeStackTop(ObjectClassMode))
205 throw sg_exception("simTime tag not inside an objectClass tag");
207 _objectClassConfigList.back()._simTimeConfig._type = getAttribute("type", atts);
209 _modeStack.push(SimTimeMode);
211 else if (strcmp(name, "position") == 0) {
212 if (!isModeStackTop(ObjectClassMode))
213 throw sg_exception("position tag not inside an objectClass tag");
215 _objectClassConfigList.back()._positionConfig._type = getAttribute("type", atts);
217 _modeStack.push(PositionMode);
219 else if (strcmp(name, "mpProperties") == 0) {
220 if (!isModeStackTop(ObjectClassMode))
221 throw sg_exception("mpProperties tag not inside an objectClass tag");
223 _objectClassConfigList.back()._mpPropertiesConfig._name = getAttribute("name", atts);
225 _modeStack.push(MPPropertiesMode);
227 else if (strcmp(name, "dataElement") == 0) {
229 DataElement dataElement;
230 dataElement._type = getAttribute("type", atts);
231 dataElement._name = getAttribute("name", atts);
232 if (isModeStackTop(ModelMode)) {
233 _objectClassConfigList.back()._modelConfig._dataElementList.push_back(dataElement);
234 } else if (isModeStackTop(SimTimeMode)) {
235 _objectClassConfigList.back()._simTimeConfig._dataElementList.push_back(dataElement);
236 } else if (isModeStackTop(PositionMode)) {
237 _objectClassConfigList.back()._positionConfig._dataElementList.push_back(dataElement);
238 } else if (isModeStackTop(ObjectClassMode)) {
239 std::string io = getAttribute("io", atts);
240 dataElement._inProperty = getAttribute("in", atts);
241 if (dataElement._inProperty.empty())
242 dataElement._inProperty = io;
243 dataElement._outProperty = getAttribute("out", atts);
244 if (dataElement._outProperty.empty())
245 dataElement._outProperty = io;
246 _objectClassConfigList.back()._dataElementList.push_back(dataElement);
248 throw sg_exception("dataElement tag not inside a position, model or objectClass tag");
251 _modeStack.push(DataElementMode);
254 else if (strcmp(name, "federateObjectModel") == 0) {
255 if (!isModeStackTop(HLAConfigurationMode))
256 throw sg_exception("federateObjectModel tag not at top level");
258 _federateObjectModel = getAttribute("name", atts);
259 _modeStack.push(FederateObjectModelMode);
263 throw sg_exception(std::string("Unknown tag ") + name);
267 virtual void data(const char * s, int length)
269 if (isModeStackTop(RTIArgumentMode)) {
270 _rtiArguments.back().append(s, length);
274 virtual void endElement(const char* name)
279 std::string getAttribute(const char* name, const XMLAttributes& atts)
281 int index = atts.findAttribute(name);
282 if (index < 0 || atts.size() <= index)
283 return std::string();
284 return std::string(atts.getValue(index));
286 std::string getAttribute(const std::string& name, const XMLAttributes& atts)
288 int index = atts.findAttribute(name.c_str());
289 if (index < 0 || atts.size() <= index)
290 return std::string();
291 return std::string(atts.getValue(index));
295 // poor peoples xml validation ...
298 HLAConfigurationMode,
309 FederateObjectModelMode
311 bool isModeStackTop(Mode mode)
313 if (_modeStack.empty())
315 return _modeStack.top() == mode;
317 std::stack<Mode> _modeStack;
319 std::string _federateObjectModel;
320 HLAVersion _rtiVersion;
321 std::list<std::string> _rtiArguments;
323 ObjectClassConfigList _objectClassConfigList;
326 class PropertyReferenceSet : public SGReferenced {
328 void insert(const std::string& relativePath, const SGSharedPtr<sg::HLAPropertyDataElement>& dataElement)
330 if (_rootNode.valid())
331 dataElement->setPropertyNode(_rootNode->getNode(relativePath, true));
332 _pathDataElementPairList.push_back(PathDataElementPair(relativePath, dataElement));
335 void setRootNode(SGPropertyNode* rootNode)
337 _rootNode = rootNode;
338 for (PathDataElementPairList::iterator i = _pathDataElementPairList.begin();
339 i != _pathDataElementPairList.end(); ++i) {
340 i->second->setPropertyNode(_rootNode->getNode(i->first, true));
343 SGPropertyNode* getRootNode()
344 { return _rootNode.get(); }
347 SGSharedPtr<SGPropertyNode> _rootNode;
349 typedef std::pair<std::string, SGSharedPtr<sg::HLAPropertyDataElement> > PathDataElementPair;
350 typedef std::list<PathDataElementPair> PathDataElementPairList;
351 PathDataElementPairList _pathDataElementPairList;
354 class AbstractSimTime : public SGReferenced {
356 virtual ~AbstractSimTime() {}
358 virtual double getTimeStamp() const = 0;
359 virtual void setTimeStamp(double) = 0;
362 class SimTimeFactory : public SGReferenced {
364 virtual ~SimTimeFactory() {}
365 virtual AbstractSimTime* createSimTime(sg::HLAAttributePathElementMap&) const = 0;
368 // A SimTime implementation that works with the simulation
369 // time in an attribute. Used when we cannot do real time management.
370 class AttributeSimTime : public AbstractSimTime {
373 virtual double getTimeStamp() const
375 virtual void setTimeStamp(double simTime)
376 { _simTime = simTime; }
378 sg::HLADataElement* getDataElement()
379 { return _simTime.getDataElement(); }
382 simgear::HLADoubleData _simTime;
385 class AttributeSimTimeFactory : public SimTimeFactory {
387 virtual AttributeSimTime* createSimTime(sg::HLAAttributePathElementMap& attributePathElementMap) const
389 AttributeSimTime* attributeSimTime = new AttributeSimTime;
390 attributePathElementMap[_simTimeIndexPathPair.first][_simTimeIndexPathPair.second] = attributeSimTime->getDataElement();
391 return attributeSimTime;
394 void setSimTimeIndexPathPair(const sg::HLADataElement::IndexPathPair& indexPathPair)
395 { _simTimeIndexPathPair = indexPathPair; }
398 sg::HLADataElement::IndexPathPair _simTimeIndexPathPair;
401 // A SimTime implementation that works with the simulation
402 // time in two attributes like the avation sim net fom works
403 class MSecAttributeSimTime : public AbstractSimTime {
405 virtual double getTimeStamp() const
407 return _secSimTime + 1e-3*_msecSimTime;
409 virtual void setTimeStamp(double simTime)
411 double sec = floor(simTime);
413 _msecSimTime = 1e3*(simTime - sec);
416 sg::HLADataElement* getSecDataElement()
417 { return _secSimTime.getDataElement(); }
418 sg::HLADataElement* getMSecDataElement()
419 { return _msecSimTime.getDataElement(); }
422 simgear::HLADoubleData _secSimTime;
423 simgear::HLADoubleData _msecSimTime;
426 class MSecAttributeSimTimeFactory : public SimTimeFactory {
428 virtual MSecAttributeSimTime* createSimTime(sg::HLAAttributePathElementMap& attributePathElementMap) const
430 MSecAttributeSimTime* attributeSimTime = new MSecAttributeSimTime;
431 attributePathElementMap[_secIndexPathPair.first][_secIndexPathPair.second] = attributeSimTime->getSecDataElement();
432 attributePathElementMap[_msecIndexPathPair.first][_msecIndexPathPair.second] = attributeSimTime->getMSecDataElement();
433 return attributeSimTime;
436 void setSecIndexPathPair(const sg::HLADataElement::IndexPathPair& indexPathPair)
437 { _secIndexPathPair = indexPathPair; }
438 void setMSecIndexPathPair(const sg::HLADataElement::IndexPathPair& indexPathPair)
439 { _msecIndexPathPair = indexPathPair; }
442 sg::HLADataElement::IndexPathPair _secIndexPathPair;
443 sg::HLADataElement::IndexPathPair _msecIndexPathPair;
446 class AbstractModel : public SGReferenced {
448 virtual ~AbstractModel() {}
450 virtual std::string getModelPath() const = 0;
451 virtual void setModelPath(const std::string&) = 0;
454 class ModelFactory : public SGReferenced {
456 virtual ~ModelFactory() {}
457 virtual AbstractModel* createModel(sg::HLAAttributePathElementMap&, bool) const = 0;
460 class AttributeModel : public AbstractModel {
462 virtual std::string getModelPath() const
463 { return _modelPath; }
464 virtual void setModelPath(const std::string& modelPath)
465 { _modelPath = modelPath; }
467 sg::HLADataElement* getDataElement()
468 { return _modelPath.getDataElement(); }
471 simgear::HLAStringData _modelPath;
474 class AttributeModelFactory : public ModelFactory {
476 virtual AttributeModel* createModel(sg::HLAAttributePathElementMap& attributePathElementMap, bool outgoing) const
478 AttributeModel* attributeModel = new AttributeModel;
479 attributePathElementMap[_modelIndexPathPair.first][_modelIndexPathPair.second] = attributeModel->getDataElement();
481 attributeModel->setModelPath(fgGetString("/sim/model/path", "default"));
482 return attributeModel;
485 void setModelIndexPathPair(const sg::HLADataElement::IndexPathPair& indexPathPair)
486 { _modelIndexPathPair = indexPathPair; }
489 sg::HLADataElement::IndexPathPair _modelIndexPathPair;
492 class AttributeMapModelFactory : public ModelFactory {
494 class Model : public AbstractModel {
496 Model(const AttributeMapModelFactory* mapModelFactory) :
497 _mapModelFactory(mapModelFactory)
500 virtual std::string getModelPath() const
502 if (_modelPath.getValue().empty())
503 return _modelPath.getValue();
504 return _mapModelFactory->mapToFlightgear(_modelPath);
506 virtual void setModelPath(const std::string& modelPath)
507 { _modelPath = _mapModelFactory->mapToExternal(modelPath); }
509 sg::HLADataElement* getDataElement()
510 { return _modelPath.getDataElement(); }
513 simgear::HLAStringData _modelPath;
514 SGSharedPtr<const AttributeMapModelFactory> _mapModelFactory;
517 virtual AbstractModel* createModel(sg::HLAAttributePathElementMap& attributePathElementMap, bool outgoing) const
519 Model* attributeModel = new Model(this);
520 attributePathElementMap[_modelIndexPathPair.first][_modelIndexPathPair.second] = attributeModel->getDataElement();
522 attributeModel->setModelPath(fgGetString("/sim/model/path", "default"));
523 return attributeModel;
526 void setModelIndexPathPair(const sg::HLADataElement::IndexPathPair& indexPathPair)
527 { _modelIndexPathPair = indexPathPair; }
529 std::string mapToFlightgear(const std::string& externalModel) const
531 std::map<std::string,std::string>::const_iterator i;
532 i = _externalToModelPathMap.find(externalModel);
533 if (i != _externalToModelPathMap.end())
537 std::string mapToExternal(const std::string& modelPath) const
539 std::map<std::string,std::string>::const_iterator i;
540 i = _modelPathToExternalMap.find(modelPath);
541 if (i != _modelPathToExternalMap.end())
543 return _externalDefault;
546 void setExternalDefault(const std::string& externalDefault)
547 { _externalDefault = externalDefault; }
548 void addExternalModelPathPair(const std::string& external, const std::string& modelPath)
550 _externalToModelPathMap[external] = modelPath;
551 _modelPathToExternalMap[modelPath] = external;
555 sg::HLADataElement::IndexPathPair _modelIndexPathPair;
556 std::map<std::string,std::string> _externalToModelPathMap;
557 std::map<std::string,std::string> _modelPathToExternalMap;
558 std::string _externalDefault;
561 // Factory class that is used to create an apternative data element for the multiplayer property attribute
562 class MPPropertyVariantDataElementFactory : public sg::HLAVariantArrayDataElement::AlternativeDataElementFactory {
564 MPPropertyVariantDataElementFactory(PropertyReferenceSet* propertyReferenceSet) :
565 _propertyReferenceSet(propertyReferenceSet)
568 virtual sg::HLADataElement* createElement(const sg::HLAVariantDataElement& variantDataElement, unsigned index)
570 const sg::HLAVariantDataType* dataType = variantDataElement.getDataType();
573 const sg::HLAEnumeratedDataType* enumDataType = dataType->getEnumeratedDataType();
576 const sg::HLADataType* alternativeDataType = dataType->getAlternativeDataType(index);
577 if (!alternativeDataType)
580 // The relative property path should be in the semantics field name
581 std::string relativePath = dataType->getAlternativeSemantics(index);
582 sg::HLAPropertyDataElement* dataElement = new sg::HLAPropertyDataElement(alternativeDataType, (SGPropertyNode*)0);
583 _propertyReferenceSet->insert(relativePath, dataElement);
588 SGSharedPtr<PropertyReferenceSet> _propertyReferenceSet;
591 class MPAttributeCallback : public sg::HLAObjectInstance::AttributeCallback {
593 MPAttributeCallback() :
594 _propertyReferenceSet(new PropertyReferenceSet),
595 _mpProperties(new sg::HLAVariantArrayDataElement)
597 _mpProperties->setAlternativeDataElementFactory(new MPPropertyVariantDataElementFactory(_propertyReferenceSet.get()));
599 virtual ~MPAttributeCallback()
602 void setLocation(sg::HLAAbstractLocation* location)
603 { _location = location; }
604 sg::HLAAbstractLocation* getLocation()
605 { return _location.get(); }
606 const sg::HLAAbstractLocation* getLocation() const
607 { return _location.get(); }
609 void setModel(AbstractModel* model)
611 AbstractModel* getModel()
612 { return _model.get(); }
613 const AbstractModel* getModel() const
614 { return _model.get(); }
616 void setSimTime(AbstractSimTime* simTime)
617 { _simTime = simTime; }
618 AbstractSimTime* getSimTime()
619 { return _simTime.get(); }
620 const AbstractSimTime* getSimTime() const
621 { return _simTime.get(); }
623 sg::HLAVariantArrayDataElement* getMPProperties() const
624 { return _mpProperties.get(); }
626 SGSharedPtr<PropertyReferenceSet> _propertyReferenceSet;
629 SGSharedPtr<sg::HLAAbstractLocation> _location;
630 SGSharedPtr<AbstractSimTime> _simTime;
631 SGSharedPtr<AbstractModel> _model;
632 SGSharedPtr<sg::HLAVariantArrayDataElement> _mpProperties;
635 class MPOutAttributeCallback : public MPAttributeCallback {
637 virtual void updateAttributeValues(sg::HLAObjectInstance&, const sg::RTIData&)
639 _simTime->setTimeStamp(globals->get_sim_time_sec());
641 SGGeod position = ifce.getPosition();
642 // The quaternion rotating from the earth centered frame to the
643 // horizontal local frame
644 SGQuatd qEc2Hl = SGQuatd::fromLonLat(position);
645 SGQuatd hlOr = SGQuatd::fromYawPitchRoll(ifce.get_Psi(), ifce.get_Theta(), ifce.get_Phi());
646 _location->setCartPosition(SGVec3d::fromGeod(position));
647 _location->setCartOrientation(qEc2Hl*hlOr);
648 // The angular velocitied in the body frame
649 double p = ifce.get_P_body();
650 double q = ifce.get_Q_body();
651 double r = ifce.get_R_body();
652 _location->setAngularBodyVelocity(SGVec3d(p, q, r));
653 // The body uvw velocities in the interface are wrt the wind instead
654 // of wrt the ec frame
655 double n = ifce.get_V_north();
656 double e = ifce.get_V_east();
657 double d = ifce.get_V_down();
658 _location->setLinearBodyVelocity(hlOr.transform(SGVec3d(n, e, d)));
660 if (_mpProperties.valid() && _mpProperties->getNumElements() == 0) {
661 if (_propertyReferenceSet.valid() && _propertyReferenceSet->getRootNode()) {
662 const sg::HLADataType* elementDataType = _mpProperties->getElementDataType();
663 const sg::HLAVariantDataType* variantDataType = elementDataType->toVariantDataType();
664 for (unsigned i = 0, count = 0; i < variantDataType->getNumAlternatives(); ++i) {
665 std::string name = variantDataType->getAlternativeSemantics(i);
666 SGPropertyNode* node = _propertyReferenceSet->getRootNode()->getNode(name);
669 _mpProperties->getOrCreateElement(count++)->setAlternativeIndex(i);
676 FlightProperties ifce;
679 class MPInAttributeCallback : public MPAttributeCallback {
681 virtual void reflectAttributeValues(sg::HLAObjectInstance& objectInstance,
682 const sg::RTIIndexDataPairList&, const sg::RTIData&)
684 // Puh, damn ordering problems with properties startup and so on
685 if (_aiMultiplayer.valid()) {
686 FGExternalMotionData motionInfo;
687 motionInfo.time = _simTime->getTimeStamp();
690 motionInfo.position = _location->getCartPosition();
691 motionInfo.orientation = toQuatf(_location->getCartOrientation());
692 motionInfo.linearVel = toVec3f(_location->getLinearBodyVelocity());
693 motionInfo.angularVel = toVec3f(_location->getAngularBodyVelocity());
694 motionInfo.linearAccel = SGVec3f::zeros();
695 motionInfo.angularAccel = SGVec3f::zeros();
697 _aiMultiplayer->addMotionInfo(motionInfo, SGTimeStamp::now().getSeconds());
700 std::string modelPath = _model->getModelPath();
701 if (modelPath.empty())
704 aiMgr = static_cast<FGAIManager*>(globals->get_subsystem("ai-model"));
708 _aiMultiplayer = new FGAIMultiplayer;
709 _aiMultiplayer->setPath(modelPath.c_str());
710 aiMgr->attach(_aiMultiplayer.get());
712 _propertyReferenceSet->setRootNode(_aiMultiplayer->getPropertyRoot());
718 if (!_aiMultiplayer.valid())
720 _aiMultiplayer->setDie(true);
725 SGSharedPtr<FGAIMultiplayer> _aiMultiplayer;
729 class MpClassCallback : public sg::HLAObjectClass::InstanceCallback {
733 virtual ~MpClassCallback()
736 virtual void discoverInstance(const sg::HLAObjectClass& objectClass, sg::HLAObjectInstance& objectInstance, const sg::RTIData& tag)
738 MPInAttributeCallback* attributeCallback = new MPInAttributeCallback;
739 objectInstance.setAttributeCallback(attributeCallback);
740 attachDataElements(objectInstance, *attributeCallback, false);
743 virtual void removeInstance(const sg::HLAObjectClass& objectClass, sg::HLAObjectInstance& objectInstance, const sg::RTIData& tag)
745 MPInAttributeCallback* attributeCallback;
746 attributeCallback = dynamic_cast<MPInAttributeCallback*>(objectInstance.getAttributeCallback().get());
747 if (!attributeCallback) {
748 SG_LOG(SG_IO, SG_WARN, "HLA: expected to have a different attribute callback in remove instance.");
751 attributeCallback->setDie();
754 virtual void registerInstance(const sg::HLAObjectClass& objectClass, sg::HLAObjectInstance& objectInstance)
756 MPOutAttributeCallback* attributeCallback = new MPOutAttributeCallback;
757 objectInstance.setAttributeCallback(attributeCallback);
758 attachDataElements(objectInstance, *attributeCallback, true);
759 attributeCallback->_propertyReferenceSet->setRootNode(fgGetNode("/", true));
762 virtual void deleteInstance(const sg::HLAObjectClass& objectClass, sg::HLAObjectInstance& objectInstance)
766 void setLocationFactory(sg::HLALocationFactory* locationFactory)
767 { _locationFactory = locationFactory; }
768 sg::HLALocationFactory* getLocationFactory()
769 { return _locationFactory.get(); }
771 void setSimTimeFactory(SimTimeFactory* simTimeFactory)
772 { _simTimeFactory = simTimeFactory; }
773 SimTimeFactory* getSimTimeFactory()
774 { return _simTimeFactory.get(); }
776 void setModelFactory(ModelFactory* modelFactory)
777 { _modelFactory = modelFactory; }
778 ModelFactory* getModelFactory()
779 { return _modelFactory.get(); }
781 void setMPPropertiesIndexPathPair(const sg::HLADataElement::IndexPathPair& indexPathPair)
782 { _mpPropertiesIndexPathPair = indexPathPair; }
783 const sg::HLADataElement::IndexPathPair& getMPPropertiesIndexPathPair() const
784 { return _mpPropertiesIndexPathPair; }
786 typedef std::map<sg::HLADataElement::Path, std::string> PathPropertyMap;
787 typedef std::map<unsigned, PathPropertyMap> AttributePathPropertyMap;
789 void setInputProperty(const sg::HLADataElement::IndexPathPair& indexPathPair, const std::string& property)
791 _inputProperties[indexPathPair.first][indexPathPair.second] = property;
793 void setOutputProperty(const sg::HLADataElement::IndexPathPair& indexPathPair, const std::string& property)
795 _outputProperties[indexPathPair.first][indexPathPair.second] = property;
799 void attachDataElements(sg::HLAObjectInstance& objectInstance, MPAttributeCallback& attributeCallback, bool outgoing)
801 sg::HLAAttributePathElementMap attributePathElementMap;
803 if (_locationFactory.valid())
804 attributeCallback.setLocation(_locationFactory->createLocation(attributePathElementMap));
805 if (_modelFactory.valid())
806 attributeCallback.setModel(_modelFactory->createModel(attributePathElementMap, outgoing));
807 if (_simTimeFactory.valid())
808 attributeCallback.setSimTime(_simTimeFactory->createSimTime(attributePathElementMap));
810 attributePathElementMap[_mpPropertiesIndexPathPair.first][_mpPropertiesIndexPathPair.second] = attributeCallback.getMPProperties();
813 attachPropertyDataElements(*attributeCallback._propertyReferenceSet,
814 attributePathElementMap, _outputProperties);
816 attachPropertyDataElements(*attributeCallback._propertyReferenceSet,
817 attributePathElementMap, _inputProperties);
819 objectInstance.setAttributes(attributePathElementMap);
821 void attachPropertyDataElements(PropertyReferenceSet& propertyReferenceSet,
822 sg::HLAAttributePathElementMap& attributePathElementMap,
823 const AttributePathPropertyMap& attributePathPropertyMap)
825 for (AttributePathPropertyMap::const_iterator i = attributePathPropertyMap.begin();
826 i != attributePathPropertyMap.end(); ++i) {
827 for (PathPropertyMap::const_iterator j = i->second.begin();
828 j != i->second.end(); ++j) {
829 sg::HLAPropertyDataElement* dataElement = new sg::HLAPropertyDataElement;
830 propertyReferenceSet.insert(j->second, dataElement);
831 attributePathElementMap[i->first][j->first] = dataElement;
836 AttributePathPropertyMap _inputProperties;
837 AttributePathPropertyMap _outputProperties;
839 SGSharedPtr<sg::HLALocationFactory> _locationFactory;
840 SGSharedPtr<SimTimeFactory> _simTimeFactory;
841 SGSharedPtr<ModelFactory> _modelFactory;
843 sg::HLADataElement::IndexPathPair _mpPropertiesIndexPathPair;
846 FGHLA::FGHLA(const std::vector<std::string>& tokens) :
847 _hlaFederate(new simgear::HLAFederate)
849 if (1 < tokens.size() && !tokens[1].empty())
850 set_direction(tokens[1]);
855 if (2 < tokens.size() && !tokens[2].empty()) {
856 std::stringstream ss(tokens[2]);
861 if (3 < tokens.size() && !tokens[3].empty())
862 _federate = tokens[3];
864 _federate = fgGetString("/sim/user/callsign");
866 if (4 < tokens.size() && !tokens[4].empty())
867 _federation = tokens[4];
869 _federation = "FlightGear";
871 if (5 < tokens.size() && !tokens[5].empty())
872 _objectModelConfig = tokens[5];
874 _objectModelConfig = "mp-aircraft.xml";
885 SG_LOG(SG_IO, SG_ALERT, "This shouldn't happen, but the channel "
886 "is already in use, ignoring");
890 if (_federation.empty()) {
891 SG_LOG(SG_IO, SG_ALERT, "No federation to join given to "
896 if (_federate.empty()) {
897 SG_LOG(SG_IO, SG_ALERT, "No federate name given to the HLA module");
901 if (!SGPath(_objectModelConfig).exists()) {
902 SGPath path(globals->get_fg_root());
904 path.append(_objectModelConfig);
906 _objectModelConfig = path.str();
908 SG_LOG(SG_IO, SG_ALERT, "Federate object model configuration \""
909 << _objectModelConfig << "\" does not exist.");
914 XMLConfigReader configReader;
916 readXML(_objectModelConfig, configReader);
917 } catch (const sg_throwable& e) {
918 SG_LOG(SG_IO, SG_ALERT, "Could not open HLA configuration file: "
922 SG_LOG(SG_IO, SG_ALERT, "Could not open HLA configuration file");
926 // Flightgears hla configuration
927 std::string objectModel = configReader.getFederateObjectModel();
928 if (objectModel.empty()) {
929 SG_LOG(SG_IO, SG_ALERT, "No federate object model given to "
933 if (!SGPath(objectModel).exists()) {
934 SGPath path(SGPath(_objectModelConfig).dir());
935 path.append(objectModel);
937 objectModel = path.str();
939 SG_LOG(SG_IO, SG_ALERT, "Federate object model file \""
940 << objectModel << "\" does not exist.");
945 // We need that to communicate to the rti
946 switch (configReader.getRTIVersion()) {
948 _hlaFederate->setVersion(simgear::HLAFederate::RTI13);
951 _hlaFederate->setVersion(simgear::HLAFederate::RTI1516);
954 _hlaFederate->setVersion(simgear::HLAFederate::RTI1516E);
957 _hlaFederate->setConnectArguments(configReader.getRTIArguments());
958 _hlaFederate->setFederationExecutionName(_federation);
959 _hlaFederate->setFederationObjectModel(objectModel);
960 _hlaFederate->setFederateType(_federate);
962 // Now that it is paramtrized, connect
963 if (!_hlaFederate->connect()) {
964 SG_LOG(SG_IO, SG_ALERT, "Could not connect to rti.");
968 // Try to create and join the new federation execution
969 if (!_hlaFederate->createJoinFederationExecution()) {
970 SG_LOG(SG_IO, SG_ALERT, "Could not join federation");
974 // bool publish = get_direction() & SG_IO_OUT;
975 // bool subscribe = get_direction() & SG_IO_IN;
977 sg::HLAFederate::ObjectModelFactory objectModelFactory;
978 if (!_hlaFederate->readObjectModelTemplate(objectModel, objectModelFactory)) {
979 SG_LOG(SG_IO, SG_ALERT, "Could not read omt file \"" << objectModel << "\"!");
983 // This should be configured form a file
984 XMLConfigReader::ObjectClassConfigList::const_iterator i;
985 for (i = configReader.getObjectClassConfigList().begin();
986 i != configReader.getObjectClassConfigList().end(); ++i) {
988 if (i->_type != "Multiplayer") {
989 SG_LOG(SG_IO, SG_ALERT, "Ignoring unknown object class type \"" << i->_type << "\"!");
993 /// The object class for HLA aircraft
994 SGSharedPtr<simgear::HLAObjectClass> objectClass;
996 // Register the object class that we need for this simple hla implementation
997 std::string aircraftClassName = i->_name;
998 objectClass = _hlaFederate->getObjectClass(aircraftClassName);
999 if (!objectClass.valid()) {
1000 SG_LOG(SG_IO, SG_ALERT, "Could not find " << aircraftClassName << " object class!");
1004 SGSharedPtr<MpClassCallback> mpClassCallback = new MpClassCallback;
1006 if (i->_positionConfig._type == "cartesian") {
1007 SGSharedPtr<sg::HLACartesianLocationFactory> locationFactory;
1008 locationFactory = new sg::HLACartesianLocationFactory;
1009 XMLConfigReader::DataElementList::const_iterator j;
1010 for (j = i->_positionConfig._dataElementList.begin();
1011 j != i->_positionConfig._dataElementList.end(); ++j) {
1013 if (j->_type == "position-x")
1014 locationFactory->setPositionIndexPathPair(0, objectClass->getIndexPathPair(j->_name));
1015 else if (j->_type == "position-y")
1016 locationFactory->setPositionIndexPathPair(1, objectClass->getIndexPathPair(j->_name));
1017 else if (j->_type == "position-z")
1018 locationFactory->setPositionIndexPathPair(2, objectClass->getIndexPathPair(j->_name));
1020 else if (j->_type == "orientation-sin-angle-axis-x")
1021 locationFactory->setOrientationIndexPathPair(0, objectClass->getIndexPathPair(j->_name));
1022 else if (j->_type == "orientation-sin-angle-axis-y")
1023 locationFactory->setOrientationIndexPathPair(1, objectClass->getIndexPathPair(j->_name));
1024 else if (j->_type == "orientation-sin-angle-axis-z")
1025 locationFactory->setOrientationIndexPathPair(2, objectClass->getIndexPathPair(j->_name));
1027 else if (j->_type == "angular-velocity-x")
1028 locationFactory->setAngularVelocityIndexPathPair(0, objectClass->getIndexPathPair(j->_name));
1029 else if (j->_type == "angular-velocity-y")
1030 locationFactory->setAngularVelocityIndexPathPair(1, objectClass->getIndexPathPair(j->_name));
1031 else if (j->_type == "angular-velocity-z")
1032 locationFactory->setAngularVelocityIndexPathPair(2, objectClass->getIndexPathPair(j->_name));
1034 else if (j->_type == "linear-velocity-x")
1035 locationFactory->setLinearVelocityIndexPathPair(0, objectClass->getIndexPathPair(j->_name));
1036 else if (j->_type == "linear-velocity-y")
1037 locationFactory->setLinearVelocityIndexPathPair(1, objectClass->getIndexPathPair(j->_name));
1038 else if (j->_type == "linear-velocity-z")
1039 locationFactory->setLinearVelocityIndexPathPair(2, objectClass->getIndexPathPair(j->_name));
1042 SG_LOG(SG_IO, SG_ALERT, "HLA: Unknown position configuration type \""
1043 << j->_type << "\"for object class \"" << aircraftClassName << "\". Ignoring!");
1047 mpClassCallback->setLocationFactory(locationFactory.get());
1048 } else if (i->_positionConfig._type == "geodetic") {
1049 SGSharedPtr<sg::HLAGeodeticLocationFactory> locationFactory;
1050 locationFactory = new sg::HLAGeodeticLocationFactory;
1052 XMLConfigReader::DataElementList::const_iterator j;
1053 for (j = i->_positionConfig._dataElementList.begin();
1054 j != i->_positionConfig._dataElementList.end(); ++j) {
1056 if (j->_type == "latitude-deg")
1057 locationFactory->setIndexPathPair(sg::HLAGeodeticLocationFactory::LatitudeDeg,
1058 objectClass->getIndexPathPair(j->_name));
1059 else if (j->_type == "latitude-rad")
1060 locationFactory->setIndexPathPair(sg::HLAGeodeticLocationFactory::LatitudeRad,
1061 objectClass->getIndexPathPair(j->_name));
1062 else if (j->_type == "longitude-deg")
1063 locationFactory->setIndexPathPair(sg::HLAGeodeticLocationFactory::LongitudeDeg,
1064 objectClass->getIndexPathPair(j->_name));
1065 else if (j->_type == "longitude-rad")
1066 locationFactory->setIndexPathPair(sg::HLAGeodeticLocationFactory::LongitudeRad,
1067 objectClass->getIndexPathPair(j->_name));
1068 else if (j->_type == "elevation-m")
1069 locationFactory->setIndexPathPair(sg::HLAGeodeticLocationFactory::ElevationM,
1070 objectClass->getIndexPathPair(j->_name));
1071 else if (j->_type == "elevation-m")
1072 locationFactory->setIndexPathPair(sg::HLAGeodeticLocationFactory::ElevationFt,
1073 objectClass->getIndexPathPair(j->_name));
1074 else if (j->_type == "heading-deg")
1075 locationFactory->setIndexPathPair(sg::HLAGeodeticLocationFactory::HeadingDeg,
1076 objectClass->getIndexPathPair(j->_name));
1077 else if (j->_type == "heading-rad")
1078 locationFactory->setIndexPathPair(sg::HLAGeodeticLocationFactory::HeadingRad,
1079 objectClass->getIndexPathPair(j->_name));
1080 else if (j->_type == "pitch-deg")
1081 locationFactory->setIndexPathPair(sg::HLAGeodeticLocationFactory::PitchDeg,
1082 objectClass->getIndexPathPair(j->_name));
1083 else if (j->_type == "pitch-rad")
1084 locationFactory->setIndexPathPair(sg::HLAGeodeticLocationFactory::PitchRad,
1085 objectClass->getIndexPathPair(j->_name));
1086 else if (j->_type == "roll-deg")
1087 locationFactory->setIndexPathPair(sg::HLAGeodeticLocationFactory::RollDeg,
1088 objectClass->getIndexPathPair(j->_name));
1089 else if (j->_type == "roll-rad")
1090 locationFactory->setIndexPathPair(sg::HLAGeodeticLocationFactory::RollRad,
1091 objectClass->getIndexPathPair(j->_name));
1092 else if (j->_type == "ground-track-deg")
1093 locationFactory->setIndexPathPair(sg::HLAGeodeticLocationFactory::GroundTrackDeg,
1094 objectClass->getIndexPathPair(j->_name));
1095 else if (j->_type == "ground-track-rad")
1096 locationFactory->setIndexPathPair(sg::HLAGeodeticLocationFactory::GroundTrackRad,
1097 objectClass->getIndexPathPair(j->_name));
1098 else if (j->_type == "ground-speed-kt")
1099 locationFactory->setIndexPathPair(sg::HLAGeodeticLocationFactory::GroundSpeedKnots,
1100 objectClass->getIndexPathPair(j->_name));
1101 else if (j->_type == "ground-speed-ft-per-sec")
1102 locationFactory->setIndexPathPair(sg::HLAGeodeticLocationFactory::GroundSpeedFtPerSec,
1103 objectClass->getIndexPathPair(j->_name));
1104 else if (j->_type == "ground-speed-m-per-sec")
1105 locationFactory->setIndexPathPair(sg::HLAGeodeticLocationFactory::GroundSpeedMPerSec,
1106 objectClass->getIndexPathPair(j->_name));
1107 else if (j->_type == "vertical-speed-ft-per-sec")
1108 locationFactory->setIndexPathPair(sg::HLAGeodeticLocationFactory::VerticalSpeedFtPerSec,
1109 objectClass->getIndexPathPair(j->_name));
1110 else if (j->_type == "vertical-speed-ft-per-min")
1111 locationFactory->setIndexPathPair(sg::HLAGeodeticLocationFactory::VerticalSpeedFtPerMin,
1112 objectClass->getIndexPathPair(j->_name));
1113 else if (j->_type == "vertical-speed-m-per-sec")
1114 locationFactory->setIndexPathPair(sg::HLAGeodeticLocationFactory::VerticalSpeedMPerSec,
1115 objectClass->getIndexPathPair(j->_name));
1117 SG_LOG(SG_IO, SG_ALERT, "HLA: Unknown position configuration type \""
1118 << j->_type << "\" for object class \"" << aircraftClassName << "\". Ignoring!");
1122 mpClassCallback->setLocationFactory(locationFactory.get());
1125 if (i->_modelConfig._type == "native") {
1126 SGSharedPtr<AttributeModelFactory> attributeModelFactory;
1127 attributeModelFactory = new AttributeModelFactory;
1129 XMLConfigReader::DataElementList::const_iterator j;
1130 for (j = i->_modelConfig._dataElementList.begin();
1131 j != i->_modelConfig._dataElementList.end(); ++j) {
1133 if (j->_type == "model-path")
1134 attributeModelFactory->setModelIndexPathPair(objectClass->getIndexPathPair(j->_name));
1137 SG_LOG(SG_IO, SG_ALERT, "HLA: Unknown model configuration type \""
1138 << j->_type << "\" for object class \"" << aircraftClassName << "\". Ignoring!");
1142 mpClassCallback->setModelFactory(attributeModelFactory.get());
1143 } else if (i->_modelConfig._type == "map") {
1145 SGSharedPtr<AttributeMapModelFactory> attributeModelFactory;
1146 attributeModelFactory = new AttributeMapModelFactory;
1148 XMLConfigReader::DataElementList::const_iterator j;
1149 for (j = i->_modelConfig._dataElementList.begin();
1150 j != i->_modelConfig._dataElementList.end(); ++j) {
1152 if (j->_type == "external")
1153 attributeModelFactory->setModelIndexPathPair(objectClass->getIndexPathPair(j->_name));
1156 SG_LOG(SG_IO, SG_ALERT, "HLA: Unknown model configuration type \""
1157 << j->_type << "\" for object class \"" << aircraftClassName << "\". Ignoring!");
1161 std::list<std::pair<std::string, std::string> >::const_iterator k;
1162 for (k = i->_modelConfig._modelMap.begin();
1163 k != i->_modelConfig._modelMap.end(); ++k) {
1165 if (k->second.empty())
1166 attributeModelFactory->setExternalDefault(k->first);
1168 attributeModelFactory->addExternalModelPathPair(k->first, k->second);
1171 mpClassCallback->setModelFactory(attributeModelFactory.get());
1174 SG_LOG(SG_IO, SG_ALERT, "HLA: Unknown model configuration type \""
1175 << i->_modelConfig._type << "\" for object class \"" << aircraftClassName << "\". Ignoring!");
1178 if (i->_simTimeConfig._type == "attribute") {
1179 SGSharedPtr<AttributeSimTimeFactory> attributeSimTimeFactory;
1180 attributeSimTimeFactory = new AttributeSimTimeFactory;
1182 XMLConfigReader::DataElementList::const_iterator j;
1183 for (j = i->_simTimeConfig._dataElementList.begin();
1184 j != i->_simTimeConfig._dataElementList.end(); ++j) {
1186 if (j->_type == "local-simtime")
1187 attributeSimTimeFactory->setSimTimeIndexPathPair(objectClass->getIndexPathPair(j->_name));
1189 SG_LOG(SG_IO, SG_ALERT, "HLA: Unknown simtime configuration type \""
1190 << j->_type << "\" for object class \"" << aircraftClassName << "\". Ignoring!");
1194 mpClassCallback->setSimTimeFactory(attributeSimTimeFactory.get());
1195 } else if (i->_simTimeConfig._type == "attribute-sec-msec") {
1196 SGSharedPtr<MSecAttributeSimTimeFactory> attributeSimTimeFactory;
1197 attributeSimTimeFactory = new MSecAttributeSimTimeFactory;
1199 XMLConfigReader::DataElementList::const_iterator j;
1200 for (j = i->_simTimeConfig._dataElementList.begin();
1201 j != i->_simTimeConfig._dataElementList.end(); ++j) {
1203 if (j->_type == "local-simtime-sec")
1204 attributeSimTimeFactory->setSecIndexPathPair(objectClass->getIndexPathPair(j->_name));
1205 else if (j->_type == "local-simtime-msec")
1206 attributeSimTimeFactory->setMSecIndexPathPair(objectClass->getIndexPathPair(j->_name));
1208 SG_LOG(SG_IO, SG_ALERT, "HLA: Unknown simtime configuration type \""
1209 << j->_type << "\" for object class \"" << aircraftClassName << "\". Ignoring!");
1213 mpClassCallback->setSimTimeFactory(attributeSimTimeFactory.get());
1215 SG_LOG(SG_IO, SG_ALERT, "HLA: Unknown simtime configuration type \""
1216 << i->_simTimeConfig._type << "\" for object class \"" << aircraftClassName << "\". Ignoring!");
1219 if (!i->_mpPropertiesConfig._name.empty()) {
1220 mpClassCallback->setMPPropertiesIndexPathPair(objectClass->getIndexPathPair(i->_mpPropertiesConfig._name));
1223 // The free configurabel property - dataElement mapping
1224 XMLConfigReader::DataElementList::const_iterator j;
1225 for (j = i->_dataElementList.begin();
1226 j != i->_dataElementList.end(); ++j) {
1228 if (j->_type == "property") {
1229 if (!j->_inProperty.empty())
1230 mpClassCallback->setInputProperty(objectClass->getIndexPathPair(j->_name), j->_inProperty);
1231 if (!j->_outProperty.empty())
1232 mpClassCallback->setOutputProperty(objectClass->getIndexPathPair(j->_name), j->_outProperty);
1234 SG_LOG(SG_IO, SG_ALERT, "HLA: Unknown dataElement configuration type \""
1235 << j->_type << "\" for object class \"" << aircraftClassName << "\". Ignoring!");
1239 objectClass->setInstanceCallback(mpClassCallback);
1241 if (i->_type == "Multiplayer")
1242 _localAircraftClass = objectClass;
1246 return is_enabled();
1255 // First push our own data so that others can recieve ...
1256 if (get_direction() & SG_IO_OUT) {
1257 if (fgGetBool("/sim/fdm-initialized", false) && _localAircraftClass.valid()) {
1258 if (!_localAircraftInstance.valid()) {
1259 _localAircraftInstance = new sg::HLAObjectInstance(_localAircraftClass.get());
1260 _localAircraftInstance->registerInstance();
1262 _localAircraftInstance->updateAttributeValues(sg::RTIData("tag"));
1266 // Then get news from others and process possible update requests
1267 if (get_direction() & (SG_IO_IN|SG_IO_OUT)) {
1268 _hlaFederate->processMessages();
1280 if (get_direction() & SG_IO_OUT) {
1281 // Remove the local object from the rti
1282 _localAircraftInstance->deleteInstance(simgear::RTIData("gone"));
1283 _localAircraftInstance = 0;
1286 // Leave the federation and try to destroy the federation execution.
1287 // Only works if no federate is joined
1288 _hlaFederate->resignDestroyFederationExecution();
1290 // throw away the HLAFederate
1291 _hlaFederate->disconnect();