#include "StallState.h"#include <iostream>#include "util/timeutil.h"#include "util/robotId.h"#include "util/robotId.h"#include "com/ipc_com.h"#define DEBUG 0static StallState *m_state_ptr = NULL;/********************************************** * msgHandler StallMessage **********************************************/static void msgHandlerStall(MSG_INSTANCE msgRef, BYTE_ARRAY callData, void *clientData){   IPC_RETURN_TYPE err = IPC_OK;   FORMATTER_PTR formatter;   rescue_stall_message msg;   formatter = IPC_msgInstanceFormatter(msgRef);   err = IPC_unmarshallData(formatter, callData, &msg,         sizeof(rescue_stall_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 STALL message from client %d : time: %ld,%ld   ",            (int)clientData, msg.robot.ts.tv_sec, msg.robot.ts.tv_usec);      fprintf(stdout, "LeftWheel: %d, RightWheel: %d\n",msg.leftWheel, msg.rightWheel);   }   if (m_state_ptr)      m_state_ptr->update(msg);}StallState::StallState() {   m_state_ptr = this;   int cur_context=0;   // Initial values   m_vars.push_back(0);   m_vars.push_back(0);   // Prepare client for reading   ComSubscribeToRobot(RESCUE_STALL_NAME, msgHandlerStall, (void *)cur_context);}void StallState::update(rescue_stall_message &msg) {   m_vars.clear();   m_vars.push_back((int) msg.leftWheel);   m_vars.push_back((int) msg.rightWheel);   timestamp = msg.robot.ts;}int StallState::getLeftStall() {   return getVar(0);}int StallState::getRightStall() {   return getVar(1);}bool StallState::getStall() {   return (getVar(1)!=0 || getVar(1)!=0);}/********************************************************************* * (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. *********************************************************************/