rfidstate.cpp

一个简单使用的控制器

#include "RFIDState.h"
#include <iostream>
#include "util/robotId.h"
#include "com/ipc_com.h"

#define DEBUG 1

static RFIDState *m_state_ptr = NULL;


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

   formatter = IPC_msgInstanceFormatter(msgRef);
   err = IPC_unmarshallData(formatter, callData, &msg,
         sizeof(rescue_geometry_message));
   IPC_freeByteArray(callData);
   if (msg.robot.id != getRobotId()) {
      if (DEBUG)
         fprintf(stdout, "Received message for other robot (%d)", msg.robot.id);
      return;
   }

   if (msg.type != RESCUE_IR_TYPE)
      return;

   if (DEBUG) {
      fprintf(stdout, "\nGot geometry message from client %d : time: %ld,%ld\n",
            (int)clientData, msg.robot.ts.tv_sec, msg.robot.ts.tv_usec);

      for (int i=0; i<msg.num; i++) {
         fprintf(stdout, "Sensor %d: Position (%f,%f,%f) and Orientation (%f,%f,%f)\n",
               i,msg.x_coords[i],msg.y_coords[i],msg.z_coords[i],
               msg.pitch_angles[i], msg.yaw_angles[i], msg.roll_angles[i]);
      }
   }

   // Init array size
   if (m_state_ptr)
      m_state_ptr->setGeometry(msg);
}



/**********************************************
 * msgHandlerRFIDMessage
 **********************************************/
static void msgHandlerRFID(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(rescue_rfid_sensor_message));
   IPC_freeByteArray(callData);
   if (msg.robot.id != getRobotId()) {
      if (DEBUG)
         fprintf(stdout, "Received message for other robot (%d)", msg.robot.id);
      return;
   }

   if (DEBUG) {
      fprintf(stdout, "\nGot RFID message from client %d : time: %ld,%ld\n",
            (int)clientData, msg.robot.ts.tv_sec, msg.robot.ts.tv_usec);
      for (int i=0; i<msg.num; i++) 
         fprintf(stdout, "Tag %d: ID: %ld\n",i,msg.id[i]);
   }

   if (m_state_ptr)
      m_state_ptr->update(msg);
}


RFIDState::RFIDState() 
{
   m_state_ptr = this;
   int cur_context=0;

   // Prepare client for reading
   ComSubscribeToRobot(RESCUE_GEOMETRY_NAME, msgHandlerGeometry, (void *)cur_context);
   ComSubscribeToRobot(RESCUE_RFID_SENSOR_NAME, msgHandlerRFID, (void *)cur_context);
}

void RFIDState::setGeometry(rescue_geometry_message &msg) 
{
   geometry = msg;
}

int RFIDState::getNumTagsInRange()
{
   return m_vars.size();
}

int RFIDState::getTagID(int num)
{
   return m_vars[num];
}

void RFIDState::update(rescue_rfid_sensor_message &msg)
{
   m_vars.clear();

   for (int i=0; i<msg.num; i++) 
      m_vars.push_back((int) msg.id[i]);
   timestamp = msg.robot.ts;
}

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