exampleGimbalPlugin

DtGimbalDriver looks for the plane element created by DtExampleDriver (or DtDriverExamplePlugin) and implements a gimbal attached to that plane at a specified offset.

It shows how to use the DtGeoReference class to position and orient the gimbal using the plane's position and orientation; and how to find azimuth and elevation angles to a target in the gimbal's local coordinate system. It attaches the observer named "Observer 1" to the gimbal, and uses the azimuth and elevation angles determined to lock the observer onto the target location.

Example Source Files

• DtGimbalDriver.h
• DtGimbalDriver.cxx

  ---

DtGimbalDriver.h

/*****************************************************************************
 * Copyright (c) 2017 MAK Technologies, Inc.
 * All rights reserved.
 *****************************************************************************/


#pragma once

#include <vrvCore/DtDriver.h>

#include <vrvUtil/DtSignalConnectionManager.h>

#include <string>


namespace makVrv
{
   class DtGeoReference;
   class DtSceneObjectAgent;
   class DtSceneObject;
}

class DtGeodeticCoord;

class DtGimbalDriver : public makVrv::DtDriver
{
public:

   DtGimbalDriver( makVrv::DtAgentManager& am );

   virtual ~DtGimbalDriver();


   virtual const std::string& className() const;

 protected:
   virtual bool onStart();

   virtual bool onStop();

   virtual bool onTick();

   virtual void attachObserverToEntity( const std::string& entityName );

   virtual void slot_displayEngineAdded( const makVrv::DtDeRecord& igRecord );

   virtual bool initialize();

   virtual void slot_coordinateSystemChanged();


private:
   DtGimbalDriver();

protected:
   makVrv::DtGeoReference* myPlaneGeoRef;
   makVrv::DtGeoReference* myGimbalGeoRef;
   makVrv::DtSceneObjectAgent* myGimbalSceneObjectAgent;
   makVrv::DtSceneObject* myPlaneSceneObject;
   makVrv::DtElementID myGimbalId;
   makVrv::DtSignalConnectionManager mySignalConnections;
   DtGeodeticCoord* myTargetLocation;
};

DtGimbalDriver.cxx

/*****************************************************************************
 * Copyright (c) 2017 MAK Technologies, Inc.
 * All rights reserved.
 *****************************************************************************/


#include "DtGimbalDriver.h"

#include <vrvCore/DtAgentManager.h>
#include <vrvCore/DtAgentUpdateResolverInterface.h>
#include <vrvCore/DtDataBank.h>
#include <vrvCore/DtDe.h>
#include <vrvCore/DtDeSharedState.h>
#include <vrvCore/DtDeSharedStateSignaler.h>
#include <vrvCore/DtDriverManager.h>
#include <vrvCore/DtElementAttributes.h>
#include <vrvCore/DtElementData.h>
#include <vrvCore/DtInputDriver.h>
#include <vrvCore/DtObserver.h>
#include <vrvCore/DtScene.h>
#include <vrvCore/DtSceneObject.h>
#include <vrvCore/DtSceneObjectAgent.hpp>

#include <vrvUtil/coordUtils.h>
#include <vrvUtil/DtCoordinateSystem.h>
#include <vrvUtil/DtGeoReference.h>

#include <vlutil/vlUtil.h>

// Use the VR-Vantage namespace.
// All classes in VR-Vantage are in this namespace.
using namespace makVrv;

const double DtTargetLatitude = DtDeg2Rad(10.0327245110);
const double DtTargetLongitude = DtDeg2Rad(60.0271819335);
const double DtTargetAltitude = 29.389;

const double DtGimbalForwardOffset = 2.0;
const double DtGimbalDownOffset = 2.5;
const double DtGimbalRightOffset = 0.;

DtGimbalDriver::DtGimbalDriver( DtAgentManager& am )
   : makVrv::DtDriver( am, "DtGimbalDriver" )
   , myPlaneGeoRef( 0 )
   , myGimbalGeoRef( 0 )
   , myGimbalSceneObjectAgent( 0 )
   , myTargetLocation( 0 )
{
   myTargetLocation = new DtGeodeticCoord( DtTargetLatitude, DtTargetLongitude, DtTargetAltitude,
      myAgentManager.de().sharedState().coordinateSystem().referenceEllipsoid() );

   mySignalConnections += DtDeSharedStateSignaler::instance( myAgentManager.de() )
      .signal_coordinateSystemChanged.connect( boost::bind(
         &DtGimbalDriver::slot_coordinateSystemChanged, this));
}

DtGimbalDriver::~DtGimbalDriver()
{
   DtGimbalDriver::onStop();
   if ( myGimbalSceneObjectAgent )
   {
      reportElementDestroyed( myGimbalId );
   }

   DtDELETE( myPlaneGeoRef );
   DtDELETE( myGimbalGeoRef );
   DtDELETE( myGimbalSceneObjectAgent );
   DtDELETE( myTargetLocation );
}

void DtGimbalDriver::slot_coordinateSystemChanged()
{
   DtDELETE( myTargetLocation );

   myTargetLocation = new DtGeodeticCoord( DtTargetLatitude, DtTargetLongitude, DtTargetAltitude,
     myAgentManager.de().sharedState().coordinateSystem().referenceEllipsoid() );
}

const std::string& DtGimbalDriver::className() const
{
   static std::string name = "DtGimbalDriver";
   return name;
}

bool DtGimbalDriver::initialize()
{
   // look up the plane from exampleDriver (or exampleDriverPlugin)
   // It has display name "Entity From Facade"
   bool found = false;

   // Iterate through all the elements
   DtElementData& elemData = myAgentManager.de().dataBank().elementData();
   DtElementData::ElementEntryMap::const_iterator curIter = 
      elemData.elements().begin();
   DtElementData::ElementEntryMap::const_iterator endIter = 
      elemData.elements().end();
   for( ; curIter != endIter; ++curIter )
   {
      DtElementEntry* entry = curIter->second;
      const DtElementAttributeDisplayName* name =
         dynamic_cast<const DtElementAttributeDisplayName*>(
         entry->findAttribute(DtElementAttributes::DisplayName));

      if ( name && name->value() == "Entity From Facade" )
      {
         // Found it.  Look up the scene object id used to visualize this element 
         // in the 3D model set.  We can use that scene object to get the position
         // and orientation of the plane
         DtElementID planeElementId = entry->elementID();
         DtElementData::SceneObjectIdList sceneObjectIds;
         elemData.getSceneObjectIds(planeElementId,
            sceneObjectIds, DtSceneObject::ModelSet_3D );

         // getSceneObjectIds() can return multiple scene object ids if multiple model sets
         // are specified.  Since we only asked for the 3D model set scene object, 
         // sceneObjectIds.size() should be 1.
         if ( sceneObjectIds.size() == 1 )
         {
            DtAgentUpdateResolverInterface* updater = agentManager().findUpdater( sceneObjectIds.front() );
            if(updater)
            {
               myPlaneSceneObject = updater->castObjectTypeFromUpdater<DtSceneObject>("DtSceneObject");
               found = true;
            }
         }
      }
   }

   // If we didn't find it, assume it hasn't been created yet. Return false, and we'll
   // try again next tick
   if (! found )
   {
      return false;
   }

   // We have the plane's scene object.  Now we can create GeoReferences for the 
   // plane and the Gimbal
   myPlaneGeoRef = new DtGeoReference( myAgentManager.de().sharedState().coordinateSystem() );
   myGimbalGeoRef = new DtGeoReference( myAgentManager.de().sharedState().coordinateSystem() );

   // We create a scene object (and associate it with an element) for the Gimbal so that 
   // our observer can be attached to it.  When an observer is attached to an element in
   // Mimic mode, it effectively uses that element's coordinate system; so attaching to
   // this Gimbal gives us an observer in Gimbal coordinates.  Note that no models are 
   // added to the scene object, so this element will be invisible (but if you had a 3D 
   // gimbal model you could create and add it here
   myGimbalSceneObjectAgent = DtSceneObjectAgent::create( myAgentManager );
   myGimbalSceneObjectAgent->setModelSet( 0 );

   // Set up the element
   myGimbalId = myAgentManager.createNewUniqueId();
   myGimbalSceneObjectAgent->setElementID( myGimbalId );
   reportElementCreated( myGimbalId, 0, DtElementEntry::Entity );

   // Give the element a name that we can use to attach to it
   DtElementAttributeDisplayName* displayName = new DtElementAttributeDisplayName( "Gimbal" );
   reportElementAttribute( myGimbalId, displayName);

   return true;
}

bool DtGimbalDriver::onStart()
{
   return true;
}

bool DtGimbalDriver::onStop()
{
   return true;
}

bool DtGimbalDriver::onTick()
{
   if ( myGimbalSceneObjectAgent == NULL )
   {
      if ( ! initialize() )
      {
         return true;
      }

      // The plane was found. Attach to it.
      attachObserverToEntity( "Gimbal" );
   }

   // Set up a GeoRef for the plane.  We use this to find the Gimbal position
   // and orientation for the Gimbal scene object (and Gimbal GeoRef)
   DtVector planePosition;
   DtTaitBryan planeOrientation;
   myPlaneSceneObject->getPosition( 0, planePosition );
   myPlaneSceneObject->getTopographicOrientation( 0, planeOrientation );
   myPlaneGeoRef->setLocationAndOrientation( DtLocalToGeodetic( planePosition,
      myAgentManager.de().sharedState().coordinateSystem() ), planeOrientation );

   // Find the position and orientation of the gimbal in geodetic coordinates, using
   // the GeoRef placed above;
   DtGeodeticCoord gimbalGeod;
   DtTaitBryan gimbalOri;
   myPlaneGeoRef->topoToGeodetic( 
      DtVector( DtGimbalForwardOffset, DtGimbalRightOffset, DtGimbalDownOffset), 
      DtTaitBryan(0., 0., 0.), 
      gimbalGeod,  gimbalOri );

   // Set up a GeoRef for our Gimbal
   myGimbalGeoRef->setLocationAndOrientation( gimbalGeod, gimbalOri );

   // Position our gimbal object
   myGimbalSceneObjectAgent->setPosition( 0, DtGeodeticToLocal( gimbalGeod, 
      myAgentManager.de().sharedState().coordinateSystem() ) );
   myGimbalSceneObjectAgent->setTopographicOrientation( 0, gimbalOri );

   // Get tower location in gimbal's topocentric system. (We don't need the tower's orientation)
   DtVector targetTopo;
   DtTaitBryan unused( 0., 0., 0. );
   myGimbalGeoRef->geodeticToTopo( *myTargetLocation, unused, targetTopo, unused );

   // find azimuth and elevation angles to our target
   double azimuthAngle = -atan2( targetTopo[1], targetTopo[0] );
   double xyPlaneDistance = sqrt( targetTopo[0] * targetTopo[0] + targetTopo[1] * targetTopo[1] );
   double elevationAngle = DtDeg2Rad( 90. ) - atan2( xyPlaneDistance, -targetTopo[2] );

   // Get the main observer
   DtObserver* observer = myAgentManager.de().driverManager().inputDriver().findObserverByName( "Observer 1" );
   if ( observer )
   {
      // Set the observer's azimuth and elevation angle to keep the tower in view
      observer->setAttachedOrientation( azimuthAngle, elevationAngle, 0. ); 
   }     

   return true;
}

void DtGimbalDriver::attachObserverToEntity( const std::string& entityName )
{
   DtObserver* observer = myAgentManager.de().
      driverManager().inputDriver().findObserverByName( "Observer 1" );
   if ( observer )
   {
      // Start by getting the existing observer state record for this observer.
      // We can modify it and then set it back.  We use the observer record
      // mechanism because it will allow the observer to attach even if the 
      // plane hasn't been created yet (the observer will retry attaching when
      // an observer record is used until the attach succeeds
      makArchives::DtObserverStateRecord observerState;
      observer->getRecord( observerState );

      // Attach to the plane
      std::set<std::string> displayNames;
      displayNames.insert( entityName );
      observerState.setAttachedToDisplayNames( displayNames );

      // We want no offset
      observerState.setObserverOffsetVector( 0., 0., 0. );

      // Get the observer settings record associated with this observer state record
      // so we can force the attach type to mimic
      makArchives::DtObserverSettingsRecord settingsRec = observerState.observerSettingsRecord();
      settingsRec.setAttachType( makArchives::DtObserverSettingsRecord::AttachTypeMimic );

      // Add the modified settings record back to the observer state record
      observerState.setObserverSettingsRecord( settingsRec );

      // Set a view magnification
      observerState.setViewMagnification( 100. );

      // ... and then add the observer state to the observer
      observer->setFromRecord( observerState );
   }
}

void DtGimbalDriver::slot_displayEngineAdded( const DtDeRecord& igRecord )
{
   stop();
   start();
}