#include "reactiveState.h"#include "com/ipc_com.h"#include "util/timeutil.h"// Minimal dstance from a tag for dropping another one#define MIN_DIST_FROM_TAG 1000 // in mm#define DEBUG_RFID 0 /********************************************** * msgHandlerRFIDSensor **********************************************/static void msgHandlerRFIDSensor(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *clientData){   IPC_RETURN_TYPE err = IPC_OK;   FORMATTER_PTR formatter;   rescue_rfid_sensor_message msg;   formatter = IPC_msgInstanceFormatter(msgRef);   err = IPC_unmarshallData(formatter, callData, &msg, sizeof(msg));   IPC_freeByteArray(callData);//   if (msg.robot.id != getRobotId()) {//      if (DEBUG_RFID)//       rlogDebug("Ignoring RFID_SENSOR_MESSAGE for other robot (%d)\n", msg.robot.id);//      return;//   }   if (DEBUG_RFID) {      rlogDebug("Received RFID_SENSOR_MESSAGE for this robot: ");      for (int i=0; i<msg.num; i++)          rlogNote( "Tag %d: ID: %ld\n",i,msg.id[i]);      rlogDebug("\n");   }   ((ReactiveState*)clientData)->updateRFIDSensor(msg);}/****************************************************************************** * msgHandlerBump * * handles reactive requests due to bumps *  * ****************************************************************************/static void msgHandlerBump(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *clientData){   (void)clientData;   IPC_RETURN_TYPE err = IPC_OK;   FORMATTER_PTR formatter;   rescue_free_bump_message msg;   formatter = IPC_msgInstanceFormatter(msgRef);   err = IPC_unmarshallData(formatter, callData, &msg,         sizeof(msg));   IPC_freeByteArray(callData);   ReactiveState *m_ptr = (ReactiveState*) clientData;   if (m_ptr)      m_ptr->update(msg);}/****************************************************************************** * msgHandlerTag * * handles reactive requests due to Tag requests  *  * ****************************************************************************/static void msgHandlerTag(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *clientData){   (void)clientData;   IPC_RETURN_TYPE err = IPC_OK;   FORMATTER_PTR formatter;   rescue_rfid_deploy_message msg;   formatter = IPC_msgInstanceFormatter(msgRef);   err = IPC_unmarshallData(formatter, callData, &msg, sizeof(msg));   IPC_freeByteArray(callData);   ReactiveState *m_ptr = (ReactiveState*) clientData;   if (m_ptr)      m_ptr->update(msg);}/****************************************************************************** * msgHandlerGradient * * handles reactive requests due to cells with positive occupancy prob *  * ****************************************************************************/static void msgHandlerGradient(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *clientData){   (void)clientData;   IPC_RETURN_TYPE err = IPC_OK;   FORMATTER_PTR formatter;   rescue_free_gradient_message msg;   formatter = IPC_msgInstanceFormatter(msgRef);   err = IPC_unmarshallData(formatter, callData, &msg,         sizeof(msg));   IPC_freeByteArray(callData);   ReactiveState *m_ptr = (ReactiveState*) clientData;   if (m_ptr)      m_ptr->update(msg);}/****************************************************************************** * ReactiveState() * * Reactive state constructor. Subscribes to the messages from VFH *  * ****************************************************************************/ReactiveState::ReactiveState(RobotAction* robot){   _robot = robot;   _bump=false;   _gradient=false;   _rfidReq=false;   _lastRFIDPos.timeStamp.tv_sec = 0;   _lastRFIDPos.timeStamp.tv_usec = 0;   _lastRFIDPosValid = false;      _nearRfid=false;      _nearRfidTime=getCurrentTime();   ComSubscribeToRobot(RESCUE_REACTIVE_FREE_GRADIENT_NAME, msgHandlerGradient, this);   ComSubscribeToRobot(RESCUE_REACTIVE_FREE_BUMP_NAME, msgHandlerBump, this);   ComSubscribeToRobot(RESCUE_RFID_DEPLOY_NAME, msgHandlerTag, this);   ComSubscribeToRobot(RESCUE_RFID_SENSOR_NAME, msgHandlerRFIDSensor,this);}bool ReactiveState::RFIDTagDetectionAtCurrentPos()   {  struct timeval now=getCurrentTime();      return _nearRfid && TimevalDiff(&now,&_nearRfidTime)<=0.5;   /*if (!_lastRFIDPosValid)      return false;   double x = _robot->getPosX();   double z = _robot->getPosZ();   double dist = hypot(x-_lastRFIDPos.x, z -_lastRFIDPos.z);   if (dist<MIN_DIST_FROM_TAG) {      if (DEBUG_RFID)	    rlogDebug("We are too close to a tag: dist=%f\n",dist);      return true;   }   return false;*/}void ReactiveState::updateRFIDSensor(rescue_rfid_sensor_message msg){   for (int i=0; i<msg.num; i++)       if(hypot(msg.x[i],msg.z[i]<MIN_DIST_FROM_TAG)  && msg.id[i]<9999)        { _nearRfid=true;        _nearRfidTime=getCurrentTime();          return;      }   _nearRfid=false;       _nearRfidTime=getCurrentTime();    /* _lastRFIDPos.x = _robot->getPosX();      _lastRFIDPos.z = _robot->getPosZ();      _lastRFIDPos.th = _robot->getPosTh();      _lastRFIDPos.timeStamp = msg.robot.ts;      _lastRFIDPosValid = true;      _lastRFIDTags.clear();   //rlogDebug("Detected RFIDS: ");   _lastRFIDTags.clear();   for (int i=0; i<msg.num; i++) {   */  /*if (DEBUG_RFID)         rlogDebug("%ld ",msg.id[i]);*/   /*   _lastRFIDTags.push_back(msg.id[i]);        }*/   //rlogDebug("\n");}void ReactiveState::update(rescue_free_bump_message msg){   _bumpGoal.x=msg.x;   _bumpGoal.z=msg.z;   _bump=true;}void ReactiveState::update(rescue_free_gradient_message msg){   _gradientGoal.x=msg.x;   _gradientGoal.z=msg.z;   _gradient=true;   _gradient_time=getCurrentTime();}void ReactiveState::update(rescue_rfid_deploy_message msg){   if (msg.triggerTag == 1)      _rfidReq=true;}/****************************************************************************** * reactiveFreeBump * * if a reactive bump msg was received sets the goal position and returns true. *  * ****************************************************************************/bool ReactiveState::reactiveFreeBump(RobotSkill::Pos& goal){   if(_bump){      goal.x=_bumpGoal.x;      goal.z=_bumpGoal.z;      _bump=false;      return true;   }   return false;}/****************************************************************************** * reactiveFreeGradient * * if a reactive gradient msg was received sets the goal position and returns true. *  * ****************************************************************************/bool ReactiveState::reactiveFreeGradient(RobotSkill::Pos& goal){      goal.x=_gradientGoal.x;      goal.z=_gradientGoal.z;      struct timeval now=getCurrentTime();      return fabs(TimevalDiff(&_gradient_time, &now))< 3;}/****************************************************************************** * requestReleaseTag * * returns true if a tag release msg was received. *  * ****************************************************************************/bool ReactiveState::requestReleaseTag(){    if(_rfidReq)   {      if(DEBUG_RFID )         rlogNote("REQUESTED TO DROP TAG ...\n");      _rfidReq=false;       if (RFIDTagDetectionAtCurrentPos()){         if(DEBUG_RFID )            rlogNote("BUT ONE IN THE WAY ...\n");         return false;      }      else{         if(DEBUG_RFID)            rlogNote("OK!\n");          return true;      }   }   return false;}/********************************************************************* * (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. *********************************************************************/