#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. *********************************************************************/