stallstate.cpp

一个简单使用的控制器

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

#define DEBUG 0

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