reactivestate.cpp

一个简单使用的控制器

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