#include "RFIDState.h"#include <iostream>#include "util/robotId.h"#include "com/ipc_com.h"#define DEBUG 1static 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. *********************************************************************/