Turn And Move To Controller

This subclass of DtTaskControllerComponent will listen to front-end tasks and implement the subtasking mechanism for a turn and move to.

---

Header file:

/*******************************************************************************
** Copyright (c) 2005 MAK Technologies, Inc.
** All rights reserved.
*******************************************************************************/
/*******************************************************************************
** $RCSfile: turnAndMoveController.h,v $ $Revision: 1.3 $ $State: Exp $
*******************************************************************************/

#ifndef turnAndMoveController_H_
#define turnAndMoveController_H_

#include "vrfobjcore/singleTaskControllerComponent.h"

class DtTurnAndMoveControllerPSR;

const char DtTurnAndMoveControllerType[] = "turn-and-move-controller";

//This example controller responds to a user task with the name "turn-and-move"
//and argument 1 specifying the name of a destination object (i.e. a waypoint).
//The task is carried out by first issuing a Turn To Heading subtask, and
//when that completes, issuing a Move To task.
//The controller registers for DtTaskCompletedReport messages from itself
//in order to find out when a subtask has completed.
class DtTurnAndMoveController : public DtSingleTaskControllerComponent
{
private:
   //Not Implemented.
   DtTurnAndMoveController();
   //Not Implemented.
   DtTurnAndMoveController(const DtTurnAndMoveController& orig);
   //Not Implemented.
   DtTurnAndMoveController& operator=(const DtTurnAndMoveController& orig);

public:

   //\copydoc DtSimComponent(const DtString&,DtVrfObject*,DtSimManager*,
   //DtComponentDescriptor*,DtReaderWriterRegistry* parentRegistry)
   DtTurnAndMoveController(const DtString& name, 
      DtVrfObject* owner,
      DtSimManager* simManager, 
      DtComponentDescriptor* desc = NULL,
      DtReaderWriterRegistry* parentRegistry = NULL);

   virtual ~DtTurnAndMoveController();

   //Initial setup of component after creation.
   virtual bool init();

   //Creates the process state repository for this component.
   virtual bool createPSR();

   //\copydoc DtSimComponent::type()
   //\return DtTurnAndMoveControllerType.
   virtual DtString type() const;

   //Overridden to register interest in user-task and task-completed
   //radio messages.
   virtual void registerTaskMsgCallbacks();

   //Callback for DtUserTask.  Calls the virtual processUserTask().
   static void userTaskCallback(DtSimMessage * msg, void * usr);

   //Starts this task.
   //Looks for DtUserTasks with name of "turn-and-move" with
   //the first argument being the name of the destination object.
   virtual void processUserTask(DtSimMessage * msg);

   //Callback for DtTaskCompletedReport.  
   //Calls the virtual processTaskCompleteReport().
   static void taskCompleteReportCallback(DtSimMessage* msg, void* usr);

   //Listens for task completed reports from the subtasks that
   //are started by this task, and issues the next subtask,
   //or completes the task as necessary when the reports arrive.
   virtual void processTaskCompleteReport(DtSimMessage* msg);

   //Main processing for component.
   virtual void tick();

protected:

   //Calculates the heading to the destination and starts a 
   //turn to heading subtask.
   virtual bool startTurningPhase();

   //Starts a move to subtask.
   virtual bool startMovingPhase();

   //Overridden for any resource cleanup when task is cleared.
   //For this example, only calls down to the base class.
   virtual void clearTask();

public:

   //\copydoc DtSimComponent(const DtString&,DtVrfObject*,DtSimManager*,
   //DtComponentDescriptor*,DtReaderWriterRegistry* parentRegistry)
   //\return New instance of this class.
   static DtSimComponent * creator(const DtString& name, 
      DtVrfObject* owner,
      DtSimManager* simManager, 
      DtComponentDescriptor* desc = 0,
      DtReaderWriterRegistry* parentRegistry = 0);

protected:
   enum DtControllerStates
   {
      StateTurning,
      StateMoving
   };

   //All state values for this controller are stored in a process state
   //repository, so that if the scenario is saved during the execution
   //of the task, it will resume correctly when the scenario is reloaded.
   //See the addPSR example for more information specifically about 
   //using process state repositories.
   DtTurnAndMoveControllerPSR* myProcessState;
};

#endif

---

Implementation:

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

#include "turnAndMoveController.h"
#include "turnAndMoveControllerPSR.h"
#include "vrfmsgs/taskMessage.h"
#include "vrfmsgs/reportMessage.h"
#include "vrftasks/userTask.h"
#include "vrftasks/turnToHeadingTask.h"
#include "vrftasks/moveToTask.h"
#include "vrftasks/taskCompleteReport.h"
#include "vrfobjcore/simRadio.h"
#include "vrftasks/radioMessageTypes.h"
#include "vrfcore/simManager.h"
#include "vrfobjcore/vrfObjectManager.h"
#include "vrfobjcore/vrfKinematicState.h"
#include "vrfobjcore/physicalWorld.h"
#include "vrfutil/kinematicTools.h"
#include "vrfobjparam/componentDescriptor.h"
#include "vrfobjcore/vrfProcessStateRepositoryManager.h"

DtTurnAndMoveController::DtTurnAndMoveController(const DtString& name, 
   DtVrfObject* owner, DtSimManager* simManager, DtComponentDescriptor* desc,
   DtReaderWriterRegistry* parentRegistry) :
   DtSingleTaskControllerComponent(name, owner, simManager, desc, parentRegistry),
   myProcessState(0)
{
}

DtTurnAndMoveController::~DtTurnAndMoveController()
{
   //Remove radio message callbacks.
   if (myRadio)
   {
      myRadio->removeRadioMessageCallback(DtUserTaskType, userTaskCallback, this);
      myRadio->removeRadioMessageCallback(DtTaskCompletedReportType, taskCompleteReportCallback, this);
   }

   //Remove and delete the PSR.
   psrManager()->removeProcessStateRepository(myProcessState);
   delete myProcessState;
   myProcessState = 0;
}

//Initial setup of component after creation.
bool DtTurnAndMoveController::init()
{
   //This will result in a call to registerTaskMsgCallbacks().
   if (!DtSingleTaskControllerComponent::init())
   {
      return false;
   }

   if (!createPSR())
   {
      return false;
   }
   return true;
}

//Creates the process state repository for this component.
//See the addPSR example for more explanation specifically on 
//creating and using process state repositories.
bool DtTurnAndMoveController::createPSR()
{
   //create the process state repository
   DtString processSRName = DtString::nullString();
   DtString processSRType = DtString::nullString();

   if (myComponentDescriptor)
   {
      processSRName = myComponentDescriptor->processStateRepositoryName();
      processSRType = myComponentDescriptor->processStateRepositoryType();
   }

   //For backwards compatibility, when name/type aren't in descriptor
   if (processSRName == DtString::nullString())
   {
      processSRName = "turn-and-move-psr";
   }

   if (processSRType == DtString::nullString())
   {
      processSRType = DtTurnAndMoveControllerPSRType;
   }

   myProcessState = static_cast<DtTurnAndMoveControllerPSR*>(
      DtVrfProcessStateRepository::createRepository(processSRName, 
      processSRType, entity()));

   if (!myProcessState)
   {
      return false;
   }

   psrManager()->addProcessStateRepository(myProcessState);

   return true;
}

//\copydoc DtSimComponent::type()
//\return DtTurnAndMoveControllerType.
DtString DtTurnAndMoveController::type() const
{
   return DtTurnAndMoveControllerType;
}

//Overridden to register interest in user-task and task-completed
//radio messages.
void DtTurnAndMoveController::registerTaskMsgCallbacks()
{
   //The top level task will be received as a DtUserTask.
   //The addTaskGUI and addTaskSim examples show how to create a 
   //new task type for both the GUI and Sim Engine.
   addTaskProcessorCallback(DtUserTaskType, 
      userTaskCallback, static_cast<void *>(this));
   //Notifications of subtasks completing will be received as
   //DtTaskCompletedReport messages.
   myRadio->addRadioMessageCallback(DtTaskCompletedReportType,
      taskCompleteReportCallback, static_cast<void*>(this));
}

void DtTurnAndMoveController::userTaskCallback(DtSimMessage * msg, void * usr)
{
   static_cast<DtTurnAndMoveController*>(usr)->processUserTask(msg);
}

//This method starts off the top level task.
void DtTurnAndMoveController::processUserTask(DtSimMessage * msg)
{
   DtTaskMessage* taskMsg = static_cast<DtTaskMessage*>(msg);
   if (taskMsg->task()->type() == DtUserTaskType)
   {
      //Only respond to user tasks specifically for this controller.
      const DtUserTask* userTask = static_cast<const DtUserTask*>(taskMsg->task());
      if (userTask->userTaskName() == "turn-and-move")
      {
         myProcessState->setDestinationName(userTask->arg1Text());
         if (!startTurningPhase())
         {
            objectConsoleWarn() <<  
               "Error starting turn-and-move task." << std::endl;
            taskComplete();
         }
      }
   }
}

void DtTurnAndMoveController::taskCompleteReportCallback(DtSimMessage* msg, void* usr)
{
   static_cast<DtTurnAndMoveController*>(usr)->processTaskCompleteReport(msg);
}

void DtTurnAndMoveController::processTaskCompleteReport(DtSimMessage* msg)
{
   DtReportMessage * rm = static_cast<DtReportMessage*>(msg);
   //We are only concerned with taskCompletedReports that come from ourself.
   if ((rm->transmitterIsEchelonId() == true && rm->sender() == entity()->echelonIdString()) ||
      (rm->transmitterIsEchelonId() == false && rm->sender() == entity()->objectName()))
   {
      DtTaskCompleteReport* report = static_cast<DtTaskCompleteReport*>(rm->report());
      //No point in doing anything if we aren't currently tasked.  Some other
      //task could be completing.
      if(tasked())
      {
         //Compare the subtask ID in the report with the one we started.
         if (myProcessState->subtaskId() == report->subtaskId())
         {
            if (myProcessState->taskState() == StateTurning)
            {
               //Turning task has completed.  
               //We must manually remove the pendingSubtask ID from the list.
               removePendingSubtaskId(report->subtaskId());
               objectConsoleInfo() << 
                  "Done turning." << std::endl;
               //Issue the moveTo subtask to start the next phase.
               startMovingPhase();
            }
            else if (myProcessState->taskState() == StateMoving)
            {
               //MoveTo task has completed.
               //We must manually remove the pendingSubtask ID from the list.
               removePendingSubtaskId(report->subtaskId());
               objectConsoleInfo() << 
                  "Done moving; Task complete." << std::endl;
               //When this subtask completes, the overall task is complete, 
               //so call taskComplete().
               taskComplete();
            }        
         }
      }
   }
}

//Main processing for component.
void DtTurnAndMoveController::tick()
{
   //This entire task is done by subtasks that are started inside
   //callback functions in response to messaged.  There is nothing
   //that needs to be done inside the tick() method.
}

bool DtTurnAndMoveController::startTurningPhase()
{
   //Lookup the target object in the object manager.
   DtVrfObject* destination = simManager()->vrfObjectManager()->
      lookupVrfObjectByName(myProcessState->destinationName());
   if (destination)
   {
      objectConsoleInfo() << 
         "Starting TURNING subtask." << std::endl;
      //Calculate the heading to the destination object.  This is the heading
      //which we will turn to face before moving.
      double desiredHeading = 
         headingAngleTo(entity()->vrfState()->vrfKinematicState()->localPosition(),
         destination->vrfState()->vrfKinematicState()->localPosition(), 
         *physicalWorld()->coordinateSystem());
      //Create a DtTurnToHeadingTask and fill in with appropriate data.
      DtTurnToHeadingTask turnTask;
      turnTask.setHeading(desiredHeading);
      //sendSubTask() will issue this new task to the entity, as well as setting
      //the subtask flag in the task to true.  It will also create a unique 
      //(within this entity) ID for the subtask and add that to the pendingSubtasks
      //list.  The ID is returned.
      int id = sendSubtask(turnTask);
      //Save the ID so that we can match it up with taskCompleted reports that
      //will be received later.
      myProcessState->setSubtaskId(id);
      //Keep track internally what phase of the overall task we are currently
      //executing.
      myProcessState->setTaskState(StateTurning);
      return true;
   }
   else
   {
      objectConsoleWarn() << 
         "Could not find destination object "
         << myProcessState->destinationName() << "." << std::endl;
      return false;
   }
}

bool DtTurnAndMoveController::startMovingPhase()
{
   objectConsoleInfo() << 
      "Starting MOVING subtask." << std::endl;
   DtMoveToTask moveToTask;
   moveToTask.setControlPtName(myProcessState->destinationName());
   //Same as in startTurningPhase().
   int id = sendSubtask(moveToTask);
   myProcessState->setSubtaskId(id);
   myProcessState->setTaskState(StateMoving);
   return true;
}

void DtTurnAndMoveController::clearTask()
{
   //This is called as a result of taskComplete(), when another controller
   //starts a task, or if the entity is issues a skipTask().
   //If this task had any local resources that should be released
   //when the task ends, that should be done here.
   //Subtasks are automatically cleared by DtTaskControllerComponent::clearTask().
   DtSingleTaskControllerComponent::clearTask();
}

DtSimComponent * DtTurnAndMoveController::creator(const DtString& name, 
   DtVrfObject* owner,
   DtSimManager* simManager, 
   DtComponentDescriptor* desc,
   DtReaderWriterRegistry* parentRegistry)
{
   return new DtTurnAndMoveController(name, owner, simManager, desc, parentRegistry);
}