vfhstate.cpp

一个简单使用的控制器

#include "VFHState.h"
#include <iostream>
#include "util/robotId.h"
#include "com/ipc_com.h"
#include "util/angles.h"
#include "util/timeutil.h"
#include "RobotSkill.h"
#include "logger/src/logger.h"
#define DEBUG (0)

static void msgHandlerVfhPieces(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *clientData)
{  
   IPC_RETURN_TYPE err = IPC_OK;
   FORMATTER_PTR formatter;
   features_vfh_pieces_message msg;

   formatter = IPC_msgInstanceFormatter(msgRef);
   err = IPC_unmarshallData(formatter, callData, &msg,sizeof(msg));
  
   IPC_freeByteArray(callData);
  
  if (msg.robot.id != getRobotId()) {
       if (DEBUG)
          rlogDebug("Ignoring PATH_MESSAGE for other robot. own id: %d, msg id: %d\n", getRobotId(), msg.robot.id);
       return;
    }

 if (DEBUG) {
      rlogDebug( "Received FEATURES_VFH_PIECES_NAME  status is %d and ok is %d \n ",err,IPC_OK);
    
   }

  ((VFHState*)clientData)->updateVfhPieces(msg);


}


VFHState::VFHState() 
{  rlogNote( "VFHState: SUBSCRIBING\n");
   ComSubscribeToRobot(FEATURES_VFH_PIECES_NAME, msgHandlerVfhPieces, this);
   _numSectors=0;
   _freespaceValues=NULL;
   _emergencyStop = 0;
   _frontClear = 1;
   _rearClear = 1;
   _anglePreference = 0;
}

VFHState::~VFHState() 
{ 
   if(_numSectors>0)
        delete []  _freespaceValues;
}


void VFHState::updateVfhPieces(features_vfh_pieces_message msg)
{  if (DEBUG)
      rlogDebug("\n\n++++++++++++++++++++Pieces %d  +++++++++++++++++++++++++\n", msg.numSectors);
   if(msg.type==DISTANCE){
        delete [] _freespaceValues;
    _numSectors= msg.numSectors;
    _freespaceValues=new int[_numSectors];
    for(int i= 0;i<msg.numSectors;i++){ 
    	_freespaceValues[i]=msg.freespaceValues[i];
        }
    }
    else rlogError("JUST DISTANCE SUPPORTED\n");

}

void VFHState::setGoal(int x, int z)
{
   _goalx=x;
   _goalz=z;
}

void VFHState::updateReactiveSensor(rescue_reactive_sensor_message msg)
{
   _frontClear = msg.frontClear;
   _rearClear = msg.rearClear;
   _emergencyStop = msg.emergencyStop;
   _anglePreference = msg.anglePreference;
}

/*********************************************************************
 * (C) Copyright 2006 Albert Ludwigs University Freiburg
 *     Institute of Computer Science
 *
 * All rights reserved. Use of this software is permitted for
 * non-commercial research purposes, and it may be copied only
 * for that use. All copies must include this copyright message.
 * This software is made available AS IS, and neither the authors
 * nor the Albert Ludwigs University Freiburg make any warranty
 * about the software or its performance.
 *********************************************************************/