?? 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. *********************************************************************/
?? 快捷鍵說明
復制代碼
Ctrl + C
搜索代碼
Ctrl + F
全屏模式
F11
切換主題
Ctrl + Shift + D
顯示快捷鍵
?
增大字號
Ctrl + =
減小字號
Ctrl + -