[TeamTalk 18]: [555] TeamTalk/Agents: Adding the TeamTalkSimulator, a high-fidelity robot simulator that uses MOAST.

[tk@edam.speech.cs.cmu.edu]

An HTML attachment was scrubbed...
URL: http://mailman.srv.cs.cmu.edu/pipermail/teamtalk-developers/attachments/20061210/b712de04/attachment-0001.html
-------------- next part --------------
Added: TeamTalk/Agents/TeamTalkSimulator/Makefile
===================================================================
--- TeamTalk/Agents/TeamTalkSimulator/Makefile	                        (rev 0)
+++ TeamTalk/Agents/TeamTalkSimulator/Makefile	2006-12-11 03:46:02 UTC (rev 555)
@@ -0,0 +1,43 @@
+MOASTLIB_DIR = /net/eucalyptus/usr0/tkharris/moast-1.2
+RCSLIB_DIR = /net/eucalyptus/usr0/tkharris/lib/rcslib
+BOEINGLIB_DIR = ../boeingLib
+PRIMITIVECOMM_DIR = ../PrimitiveComm
+
+DEFS = -DDEFAULT_NML_FILE=\"$(MOASTLIB_DIR)/etc/moast.nml\" \
+ -DDEFAULT_INI_FILE=\"$(MOASTLIB_DIR)/etc/moast.nml\"
+INCLUDES = -I. -I$(MOASTLIB_DIR)/include -I$(RCSLIB_DIR)/include \
+ -I$(PRIMITIVECOMM_DIR) -I$(BOEINGLIB_DIR)/boeing
+LDFLAGS = -L$(MOASTLIB_DIR)/lib -L$(RCSLIB_DIR)/lib
+LDLIBS = -lmoast -lrcs -lposemath -lm #-lwsock32
+
+CPPFLAGS = $(DEFS) $(INCLUDES)
+CXXFLAGS = -g -Wall -O0
+CXXCOMPILE = $(CXX)  -c $(CPPFLAGS) $(CXXFLAGS)
+CXXLINK = $(CXX) $(CXXFLAGS) $(LDFLAGS) -o $@
+
+OBJECTS = TeamTalkSimulator.o robot.o imageClient.o \
+ $(PRIMITIVECOMM_DIR)/libboeingrobotserver.a
+
+EXECUTABLES = TeamTalkSimulator
+
+all: $(EXECUTABLES)
+
+TeamTalkSimulator: primitivecomm $(OBJECTS)
+	@rm -f TeamTalkSimulatedBot
+	$(CXXLINK) $(OBJECTS) $(LDLIBS)
+
+primitivecomm:
+	$(MAKE) -C $(PRIMITIVECOMM_DIR)
+
+TeamTalkSimulator.o: TeamTalkSimulator.cc robot.h imageClient.h $(PRIMITIVECOMM_DIR)/utils.h
+	$(CXXCOMPILE) -o $@ $<
+
+robot.o: robot.cc robot.h imageClient.h $(PRIMITIVECOMM_DIR)/utils.h
+	$(CXXCOMPILE) -o $@ $<
+
+imageClient.o: imageClient.cc imageClient.h $(PRIMITIVECOMM_DIR)/utils.h
+	$(CXXCOMPILE) -o $@ $<
+
+clean:
+	$(MAKE) -C $(PRIMITIVECOMM_DIR) clean
+	rm -f *.o $(EXECUTABLES)


Property changes on: TeamTalk/Agents/TeamTalkSimulator/Makefile
___________________________________________________________________
Name: svn:executable
   + 

Added: TeamTalk/Agents/TeamTalkSimulator/TeamTalkSimulator.cc
===================================================================
--- TeamTalk/Agents/TeamTalkSimulator/TeamTalkSimulator.cc	                        (rev 0)
+++ TeamTalk/Agents/TeamTalkSimulator/TeamTalkSimulator.cc	2006-12-11 03:46:02 UTC (rev 555)
@@ -0,0 +1,414 @@
+/** TeamTalkSimulatedBot
+ * Author: TK Harris <tkharris at cs.cmu.edu>
+ *
+ * Simulated Boeing robots in the usarsim environment.
+ *
+ * Created: 2006/9/11
+ **/
+
+#include <iostream>
+#include <sstream>
+#include <fstream>  //for initial map kludge
+
+#include <getopt.h>
+#include <pthread.h>
+#include <signal.h>
+
+using namespace std;
+
+#define DEBUG 1
+
+#include "robot.h"
+#include <utils.h>
+#include <boeing_trader_server.h>
+#include <boeing_robot_server.h>
+#include <boeing_map_server.h>
+
+#include <rcs.hh>
+#include "moastTypes.hh"	// MOAST_NML_BUFFER_NAME_LEN
+#include "moastNmlOffsets.hh"	// NAV_DATA_BASE, etc.
+#include "sectMobPL.hh"		// secMobPL_format()
+
+static RCS_CMD_CHANNEL *sectMobPLCmdBuf = NULL;
+static RCS_STAT_CHANNEL *sectMobPLStatBuf = NULL;
+static RCS_CMD_CHANNEL *sectMobPLCfgBuf = NULL;
+static RCS_STAT_CHANNEL *sectMobPLSetBuf = NULL;
+static SectMobPLStat *sectMobPLStatPtr;
+static SectMobPLSet *sectMobPLSetPtr;
+
+map<string, Robot*> robots;
+
+#define MOAST_DIR "/net/eucalyptus/usr0/tkharris/moast-1.2/"
+#define MOAST_BIN "/net/eucalyptus/usr0/tkharris/moast-1.2/bin"
+#ifndef DEFAULT_NML_FILE
+#define DEFAULT_NML_FILE "/net/eucalyptus/usr0/tkharris/moast-1.2/etc/moast.nml"
+#endif
+#ifndef DEFAULT_INI_FILE
+#define DEFAULT_INI_FILE "/net/eucalyptus/usr0/tkharris/moast-1.2/etc/moast.ini"
+#endif
+
+#define THIS_PROCESS "sectShell"
+
+char nml_file_default[] = DEFAULT_NML_FILE;
+char *nml_file_env;
+char *NML_FILE = NULL;
+char ini_file_default[] = DEFAULT_INI_FILE;
+char *ini_file_env;
+char *INI_FILE = NULL;
+int bufnum = 1;
+
+/****************** MAP STUFF *********************/
+
+static unsigned char* conveyedMap;
+static unsigned char* currentMap;
+static unsigned char* diffMap;
+static int mapWidth, mapHeight;
+static int mapXOffset, mapYOffset;
+
+
+void readMapFile() {
+  //we're going to do this a lot, so just open once
+  
+}
+
+void sendFullMap(Boeing::MapServer *m) {
+  readMapFile();
+  Boeing::MsgMap::MsgMapFactory(Boeing::MAP_RLE, 
+				msgMap, mapData, mapWidth*mapHeight,
+				0, 0, mapWidth, mapHeight); 
+  cerr << "raw: " << msgMap->map[0]
+       << " rl: " << ((msgMap->map[0]&0xFFFFFF00)>>8) 
+       << " rv: " << (msgMap->map[0]&0x000000FF) << endl;
+  mserver->sendFullMap(msgMap);
+}
+
+void sendDiffMap(Boeing::MapServer *m) {
+  readMapFile();
+  Boeing::MsgMap::MsgMapFactory(Boeing::MAP_RLE,
+				msgMap, diff_map, 
+				0, 0, mapWidth, mapHeight); 
+  cerr << "raw: " << msgMap->map[0]
+       << " rl: " << ((msgMap->map[0]&0xFFFFFF00)>>8) 
+       << " rv: " << (msgMap->map[0]&0x000000FF) << endl;
+  mserver->sendDiffMap(msgMap);
+}
+
+void* readMapRequests(void *m) {
+  bool subscribed = false;
+  int updatePeriod = 1;
+
+  Boeing::MapServer *mserver = (Boeing::MapServer *) m;
+  for(;;) {
+    const Boeing::MsgMapServer *msg = mserver->getNextMessage();
+    if(msg) {
+      switch(msg->hdr.type) {
+      case Boeing::MAP_SUBSCRIBE:
+	sendFullMap(m);
+	subscribed = true;
+	break;
+      case Boeing::MAP_KEEPALIVE: 
+	break;
+      case Boeing::MAP_ACK:
+	break;
+      case Boeing::MAP_UNSUBSCRIBE: 
+	subscribed = false;
+	break;
+      default: 
+	cerr << "unknown or unhandled msgtype: " << msg->hdr.type << endl;
+      }
+    } else {
+      if(subscribed) sendDiffMap(m);
+      sleep(updatePeriod);
+    }
+  }
+}
+
+/*************************** END MAP STUFF ****************************/
+
+void* readTrades(void *t) {
+  Boeing::TraderServer *tserver = (Boeing::TraderServer *) t;
+  for(;;) {
+    const Boeing::MsgTraderServer *msg = tserver->getNextMessage();
+    if(!msg) {
+      //cerr << "getNextMessage is NULL" << endl;
+      sleep(1000);
+      continue;
+    }
+
+    sectMobPLStatBuf->read();
+    sectMobPLSetBuf->read();    
+
+    if(msg->hdr.type != Boeing::TRADER_TASK) {
+      cerr << "don't know what to do with header type " << msg->hdr.type << endl;
+      continue;
+    }
+    cerr << "got: " << msg->msg_task.task << endl;
+    istringstream task(msg->msg_task.task);
+    string token;
+    task >> token;
+    if(token != "search") {
+      cerr << "don't know how to deal with command " << token << endl;
+      continue;
+    }
+    float x, y;
+    while(task >> x) {
+      if(!(task >> y)) break;
+      for(map<string, Robot*>::iterator i = robots.begin(); i != robots.end(); i++) {
+	if(i->first == "bashful") i->second->setFollow("clyde");
+	else if(i->first == "clyde") i->second->stackGoal(x, y);
+      }
+    }
+  }
+}
+
+void readMapData() {
+  ifstream f("stubdb.txt");
+  f >> mapWidth >> mapHeight;
+  mapData = new unsigned char[mapWidth*mapHeight];
+  for(int j=0; j<mapHeight; j++) {
+   for(int i=0; i<mapWidth; i++) {
+      int n;
+      f >> n;
+      mapData[j*mapWidth+i] = (unsigned char)n;
+    }
+  }
+}
+
+static int NmlSvrPid = 0;
+static int SectMobPLPid = 0;
+static void sigint_handler(int sig) {
+  for(map<string, Robot*>::const_iterator i=robots.begin(); i!=robots.end(); i++) {
+    delete i->second; //deleting the robot kills the asociated moast procs
+  }
+  if(SectMobPLPid) kill(SectMobPLPid, SIGINT);
+  if(NmlSvrPid) kill(NmlSvrPid, SIGINT);
+  signal(SIGINT, SIG_DFL); //reset sigint handler
+  kill(getpid(), SIGINT);  //kill yourself
+}
+
+static int cleanupSectMobPLNmlBuffers(void)
+{
+  if (NULL != sectMobPLCmdBuf)
+    delete sectMobPLCmdBuf;
+  if (NULL != sectMobPLStatBuf)
+    delete sectMobPLStatBuf;
+  if (NULL != sectMobPLSetBuf)
+    delete sectMobPLSetBuf;
+  if (NULL != sectMobPLCfgBuf)
+    delete sectMobPLCfgBuf;
+
+  sectMobPLStatBuf = NULL;
+  sectMobPLSetBuf = NULL;
+  sectMobPLCmdBuf = NULL;
+  sectMobPLCfgBuf = NULL;
+
+  return 0;
+}
+
+static void mapDump() {
+  sectMobPLStatBuf->read();
+  sectMobPLSetBuf->read();
+
+  SectMobPLCfgDumpWM dump = SectMobPLCfgDumpWM();
+  dump.turnOn = 1;
+  dump.serial_number = sectMobPLSetPtr->echo_serial_number + 1;
+  dump.skip = 5;
+  dump.dumpContinuous = true;
+  strcpy(dump.fileName, "/tmp/SectMobPLDispOutput.ppm");
+  sectMobPLCfgBuf->write(&dump);
+}
+
+static int initSectMobPLNmlBuffers(char *config_file, int bufnum) {
+  cleanupSectMobPLNmlBuffers();
+
+  {
+    ostringstream chanName;
+    chanName << SECT_MOB_PL_CMD_NAME << bufnum;
+    sectMobPLCmdBuf =
+      new RCS_CMD_CHANNEL(sectMobPL_format, chanName.str().c_str(),
+			  THIS_PROCESS, config_file);
+    if (!sectMobPLCmdBuf->valid()) {
+      delete sectMobPLCmdBuf;
+      sectMobPLCmdBuf = 0;
+      return 1;
+    }
+  }
+
+  {
+    ostringstream chanName;
+    chanName << SECT_MOB_PL_STAT_NAME << bufnum;
+    sectMobPLStatBuf =
+      new RCS_STAT_CHANNEL(sectMobPL_format, chanName.str().c_str(),
+			   THIS_PROCESS, config_file);
+    if (!sectMobPLStatBuf->valid()) {
+      delete sectMobPLStatBuf;
+      sectMobPLStatBuf = 0;
+      return 1;
+    }
+  }
+
+  {
+    ostringstream chanName;
+    chanName << SECT_MOB_PL_CFG_NAME << bufnum;
+    sectMobPLCfgBuf =
+      new RCS_CMD_CHANNEL(sectMobPL_format, chanName.str().c_str(),
+			  THIS_PROCESS, config_file);
+    if (!sectMobPLCfgBuf->valid()) {
+      delete sectMobPLCfgBuf;
+      sectMobPLCfgBuf = 0;
+      return 1;
+    }
+  }
+
+  {
+    ostringstream chanName;
+    chanName << SECT_MOB_PL_CMD_NAME << bufnum;
+    sectMobPLSetBuf =
+      new RCS_STAT_CHANNEL(sectMobPL_format, chanName.str().c_str(),
+			   THIS_PROCESS, config_file);
+    if (!sectMobPLSetBuf->valid()) {
+      delete sectMobPLSetBuf;
+      sectMobPLSetBuf = 0;
+      return 1;
+    }
+  }
+
+  return 0;
+}
+
+int initSectionControl() {
+  if (NULL == NML_FILE) {
+    nml_file_env = getenv("CONFIG_NML");
+    if (NULL != nml_file_env) {
+      NML_FILE = nml_file_env;
+    } else {
+      NML_FILE = nml_file_default;
+    }
+  }
+
+  // ditto for the .ini file
+  if (NULL == INI_FILE) {
+    ini_file_env = getenv("CONFIG_INI");
+    if (NULL != ini_file_env) {
+      INI_FILE = ini_file_env;
+    } else {
+      INI_FILE = ini_file_default;
+    }
+  }
+
+  // initial allocate all the buffers
+  int retval = initSectMobPLNmlBuffers(NML_FILE, bufnum);
+
+  if (retval) {
+    cerr << "TeamTalkSimulator: Can't allocate NML buffers" << endl;
+    return 1;
+  }
+
+  // set up our convenient pointer to status and settings
+  sectMobPLStatPtr = (SectMobPLStat *) sectMobPLStatBuf->get_address();
+  sectMobPLSetPtr = (SectMobPLSet *) sectMobPLSetBuf->get_address();  
+  
+  sectMobPLStatBuf->read();
+  sectMobPLSetBuf->read();
+
+  // set up init command
+  SectMobPLCmdInit sectInit = SectMobPLCmdInit();
+  sectInit.serial_number = sectMobPLStatPtr->echo_serial_number + 1;
+  sectInit.vehList_length = robots.size();
+  for(int i=0; i<sectInit.vehList_length; i++) {
+    sectInit.vehList[i] = i+1;
+  }
+  sectMobPLCmdBuf->write(&sectInit);
+
+  return 0;
+}
+
+int main(int argc, char *argv[])
+{
+  //register sigint handler
+  signal(SIGINT, sigint_handler);
+
+  //clear ipc and start the moast nml server
+  spawn(true, MOAST_BIN, "ipc-clear");
+  NmlSvrPid = spawn(false, MOAST_BIN, "moastNmlSvr");
+  sleep(3);
+  SectMobPLPid = spawn(false, MOAST_BIN, "sectMobPL");
+
+  string name = "tester";
+  string peerfile;
+  for(int i=1; i<argc; i++) {
+    //if(!strcmp(argv[i], "--peer"))
+    //	RobotServer::BROADCASTADDR = argv[++i];
+    if(!strcmp(argv[i], "--name"))
+      name = argv[++i];
+    else if(!strcmp(argv[i], "--peerfile"))
+      peerfile = argv[++i];
+    else {
+      //start a robot, put it on the stack
+      //robots[argv[i]] = new Robot(&robots, argv[i], atoi(argv[i]), 1);
+    }
+  }
+  
+  if(!peerfile.empty()) {
+    int robot_count = 1;
+    cerr << "adding robots from " << peerfile << endl;
+    ifstream Frobotips(peerfile.c_str());
+    if(!Frobotips) {
+      cerr << "problem opening " << peerfile << endl;
+      throw exception();
+    }
+    string rname;
+    while(Frobotips >> rname) {
+      if(rname.at(0) == '#') {
+	ignoreToEndOfLine(Frobotips);
+	continue;
+      }
+      toupper(rname);
+      if(rname == "MAPSERVER" || rname == "OPTRADER") {
+	ignoreToEndOfLine(Frobotips);
+	continue;
+      }
+      string rip;
+      Frobotips >> rip;
+      string::size_type i = rip.find(':');
+      if(i != string::npos) {
+	unsigned short port = atoi(string(rip, i+1, rip.size()-i-1).c_str());
+	robots[rname] = new Robot(&robots, rname, robot_count++, port);
+	cerr << "added " << rname << '@' << port << endl;
+      } else {
+	robots[rname] = new Robot(&robots, rname, robot_count++);
+	cerr << "added " << rname << endl;
+      }
+      ignoreToEndOfLine(Frobotips);
+    }
+  }
+  if(robots.empty()) robots[name] = new Robot(&robots, name, 1);
+  
+  //initialize section control
+  initSectionControl();
+  //start dumping map
+  mapDump();
+
+  //start the trader
+  Boeing::TraderServer tserver;
+  tserver.open();
+  //handle trading messages
+  pthread_t tradeThread_;
+  pthread_create(&tradeThread_, NULL, readTrades, (void *) &tserver);
+
+  readMapData();
+  //start the map server
+  Boeing::MapServer mserver;
+  mserver.open();
+  //handle trading messages
+  pthread_t mapThread_;
+  pthread_create(&mapThread_, NULL, readMapRequests, (void *) &mserver);
+
+  //wait for all of the threads to terminate
+  cerr << "wait for thread termination...\n" << endl;
+  pthread_join(tradeThread_, NULL);
+  pthread_join(mapThread_, NULL);
+  //should probably also be waiting for robot server handles here
+  return 0;
+
+}


Property changes on: TeamTalk/Agents/TeamTalkSimulator/TeamTalkSimulator.cc
___________________________________________________________________
Name: svn:executable
   + 

Added: TeamTalk/Agents/TeamTalkSimulator/imageClient.cc
===================================================================
--- TeamTalk/Agents/TeamTalkSimulator/imageClient.cc	                        (rev 0)
+++ TeamTalk/Agents/TeamTalkSimulator/imageClient.cc	2006-12-11 03:46:02 UTC (rev 555)
@@ -0,0 +1,139 @@
+#include <netdb.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <sys/socket.h>
+#include <unistd.h>
+
+#include <iostream>
+
+#include <utils.h>
+
+#include "imageClient.h"
+
+using namespace std;
+
+ImageClient::ImageClient(const string& host, unsigned short port) 
+  : sockfd(0), image_(NULL), image_size_(0) 
+{
+  struct sockaddr_in serv_addr;
+  struct hostent *hostptr;
+
+  if(!(hostptr = gethostbyname(host.c_str()))) {
+    cerr << "socket error: cannot find host " << host << endl;
+    return;
+  }
+
+  memset((char *) &serv_addr, 0, sizeof(serv_addr));
+  serv_addr.sin_family = AF_INET;
+  memcpy((char *) &serv_addr.sin_addr, 
+	 hostptr->h_addr_list[0], 
+	 hostptr->h_length);
+  serv_addr.sin_port = htons(port);
+
+  if ((sockfd = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
+    perror("socket");
+    return;
+  }
+
+  if (connect(sockfd, (struct sockaddr*) &serv_addr, sizeof(serv_addr)) < 0) {
+    perror("connect");
+    return;
+  }
+
+  //getImage_();
+}
+
+ImageClient::ImageClient(const string& host, unsigned short port,
+			 int* image_size, const unsigned char*& image) 
+  : sockfd(0), image_(NULL), image_size_(0) 
+{
+  struct sockaddr_in serv_addr;
+  struct hostent *hostptr;
+
+  if(!(hostptr = gethostbyname(host.c_str()))) {
+    cerr << "socket error: cannot find host " << host << endl;
+    return;
+  }
+
+  memset((char *) &serv_addr, 0, sizeof(serv_addr));
+  serv_addr.sin_family = AF_INET;
+  memcpy((char *) &serv_addr.sin_addr, 
+	 hostptr->h_addr_list[0], 
+	 hostptr->h_length);
+  serv_addr.sin_port = htons(port);
+
+  if ((sockfd = socket(AF_INET, SOCK_STREAM, 0)) < 0) {
+    perror("socket");
+    return;
+  }
+
+  //cerr << "about to connect" << endl;
+  if (connect(sockfd, (struct sockaddr*) &serv_addr, sizeof(serv_addr)) < 0) {
+    perror("connect");
+    return;
+  }
+  //cerr << "connected" << endl;
+
+  //if((*image_size = getImage_()) > 0) image = image_;
+}
+
+ImageClient::~ImageClient() {
+  delete image_;
+  close(sockfd);
+}
+
+int ImageClient::getImage_() {
+  char iType;
+  if(!readn(&iType)) {  
+    cerr << "problem getting image type" << endl;
+    return -1;
+  }
+  //cerr << "Imgage Type is " << (int)iType << endl;
+  if((int)iType < 1 || (int)iType > 5) {
+    cerr << "can't deal with image of type " << (int)iType << endl;
+    return -1;
+  }
+  char iName[16];
+  if(!readn(&iName)) {
+    cerr << "problem reading image name" << endl;
+    return -1;
+  }
+  //cerr << "image name is " << iName << endl;
+  if(!readn(&image_size_)) {
+    cerr << "problem getting image type" << endl;
+    return -1;
+  }
+  image_size_ = ntohl(image_size_);
+  //cerr << "image size is " << image_size_ << endl;
+  if(image_size_ <= 0) {
+    cerr << "bad image size: " << image_size_ << endl;
+    return -1;
+  }
+
+  if(image_ == NULL) {
+    image_ = (unsigned char*)malloc(max_image_size_ = image_size_);
+  }
+  else if(image_size_ > max_image_size_) {
+    image_ = (unsigned char*)realloc(image_, max_image_size_ = image_size_);
+  }
+  if(!readn(image_, image_size_)) {
+    cerr << "problem getting image" << endl;
+    return -1;
+  }
+  return image_size_;
+}
+
+int ImageClient::getImage(const unsigned char*& imagePtr) {
+  char ack[] = {'O', 'k', '\n', '\r'};
+  //cerr << "sending" << endl;
+  send(sockfd, ack, 4, 0);
+
+  if(getImage_() <= 0) {
+    cerr << "cannot get image" << endl;
+    return -1;
+  }
+  imagePtr = image_;
+  return image_size_;
+}
+


Property changes on: TeamTalk/Agents/TeamTalkSimulator/imageClient.cc
___________________________________________________________________
Name: svn:executable
   + 

Added: TeamTalk/Agents/TeamTalkSimulator/imageClient.h
===================================================================
--- TeamTalk/Agents/TeamTalkSimulator/imageClient.h	                        (rev 0)
+++ TeamTalk/Agents/TeamTalkSimulator/imageClient.h	2006-12-11 03:46:02 UTC (rev 555)
@@ -0,0 +1,41 @@
+#ifndef IMAGE_CLIENT_H
+#define IMAGE_CLIENT_H
+
+#include <string>
+
+#include <utils.h>
+
+class ImageClient {
+ protected:
+  int sockfd;
+  unsigned char* image_;
+  int image_size_;
+  int max_image_size_;
+  
+  template<class C> bool readn(C* datum, int n=1) const {
+    if(n <= 0) return true;
+    int s = n*sizeof(C);
+    int i = 0;
+    int rv;
+    do {
+      i += rv = recv(sockfd, ((char*)datum)+i, s-i, 0);
+      //cerr << "read: " << rv << " bytes" << endl;
+      if(rv < 0) {
+	perror("recv");
+	return false;
+      }
+    } while(i<s);
+    //if(sizeof(C) == 4) for(int j=0; j<n; j++) datum[j] = ntohl(datum[j]);
+    return true;
+  };
+
+  int getImage_();
+ public:
+  ImageClient(const string& host, unsigned short port, int* image_size, const unsigned char*& image);
+  ImageClient(const string& host, unsigned short port);
+  ~ImageClient();
+
+  int getImage(const unsigned char*& imagePtr);
+};
+
+#endif


Property changes on: TeamTalk/Agents/TeamTalkSimulator/imageClient.h
___________________________________________________________________
Name: svn:executable
   + 

Added: TeamTalk/Agents/TeamTalkSimulator/qSimulator.sh
===================================================================
--- TeamTalk/Agents/TeamTalkSimulator/qSimulator.sh	                        (rev 0)
+++ TeamTalk/Agents/TeamTalkSimulator/qSimulator.sh	2006-12-11 03:46:02 UTC (rev 555)
@@ -0,0 +1,5 @@
+#!/bin/bash
+
+rm -f runSimulator.sh.*
+
+qsub runSimulator.sh


Property changes on: TeamTalk/Agents/TeamTalkSimulator/qSimulator.sh
___________________________________________________________________
Name: svn:executable
   + 

Added: TeamTalk/Agents/TeamTalkSimulator/robot.cc
===================================================================
--- TeamTalk/Agents/TeamTalkSimulator/robot.cc	                        (rev 0)
+++ TeamTalk/Agents/TeamTalkSimulator/robot.cc	2006-12-11 03:46:02 UTC (rev 555)
@@ -0,0 +1,632 @@
+#include <cmath>
+//#include <process.h>
+
+#include <iostream>
+#include <fstream>
+#include <sstream>
+
+#include <signal.h>
+
+#define DEBUG 1
+#include "robot.h"
+
+#include <utils.h>
+
+#define MOAST_DIR "/net/eucalyptus/usr0/tkharris/moast-1.2"
+#define MOAST_BIN "/net/eucalyptus/usr0/tkharris/moast-1.2/bin"
+#ifndef DEFAULT_NML_FILE
+#define DEFAULT_NML_FILE "/net/eucalyptus/usr0/tkharris/moast-1.2/etc/moast.nml"
+#endif
+#ifndef DEFAULT_INI_FILE
+#define DEFAULT_INI_FILE "/net/eucalyptus/usr0/tkharris/moast-1.2/etc/moast.ini"
+#endif
+
+#define THIS_PROCESS "vehShell"
+
+using namespace std;
+
+#ifndef PI
+const double PI(4.0 * atan2(1.0, 1.0));
+#endif
+
+const unsigned int shift = 6;
+const unsigned int mask = ~0U << (sizeof(unsigned int)*8 - shift);
+
+int Robot::cleanupVehMobPLNmlBuffers(void)
+{
+  if (NULL != vehMobPLCmdBuf)
+    delete vehMobPLCmdBuf;
+  if (NULL != vehMobPLStatBuf)
+    delete vehMobPLStatBuf;
+  if (NULL != vehMobPLSetBuf)
+    delete vehMobPLSetBuf;
+  if (NULL != vehMobPLCfgBuf)
+    delete vehMobPLCfgBuf;
+
+  vehMobPLStatBuf = NULL;
+  vehMobPLSetBuf = NULL;
+  vehMobPLCmdBuf = NULL;
+  vehMobPLCfgBuf = NULL;
+
+  return 0;
+}
+
+int Robot::initVehMobPLNmlBuffers(char *config_file, int bufnum)
+{
+  print_debugln("cleaning up nmlbuffers");
+  cleanupVehMobPLNmlBuffers();
+
+  {
+    ostringstream chanName;
+    chanName << VEH_MOB_PL_CMD_NAME << bufnum;
+    vehMobPLCmdBuf =
+      new RCS_CMD_CHANNEL(vehMobPL_format, chanName.str().c_str(), 
+			  THIS_PROCESS, config_file);
+  }
+  if (!vehMobPLCmdBuf->valid()) {
+    delete vehMobPLCmdBuf;
+    vehMobPLCmdBuf = 0;
+    return 1;
+  }
+
+  {
+    ostringstream chanName;
+    chanName << PRIM_MOB_JA_CMD_NAME << bufnum;
+    primMobJACmdBuf =
+      new RCS_CMD_CHANNEL(primMobJA_format, chanName.str().c_str(), 
+			  THIS_PROCESS, config_file);
+  }
+  if (!primMobJACmdBuf->valid()) {
+    delete primMobJACmdBuf;
+    primMobJACmdBuf = 0;
+    return 1;
+  }
+
+  {
+    ostringstream chanName;
+    chanName << VEH_MOB_PL_STAT_NAME << bufnum;
+    vehMobPLStatBuf =
+      new RCS_STAT_CHANNEL(vehMobPL_format, chanName.str().c_str(),
+			   THIS_PROCESS, config_file);
+  }
+  if (!vehMobPLStatBuf->valid()) {
+    delete vehMobPLStatBuf;
+    vehMobPLStatBuf = 0;
+    return 1;
+  }
+
+  {
+    ostringstream chanName;
+    chanName << PRIM_MOB_JA_STAT_NAME << bufnum;
+    primMobJAStatBuf =
+      new RCS_STAT_CHANNEL(primMobJA_format, chanName.str().c_str(),
+			   THIS_PROCESS, config_file);
+  }
+  if (!primMobJAStatBuf->valid()) {
+    delete primMobJAStatBuf;
+    primMobJAStatBuf = 0;
+    return 1;
+  }
+
+  {
+    ostringstream chanName;
+    chanName << VEH_MOB_PL_CFG_NAME << bufnum;
+    vehMobPLCfgBuf =
+      new RCS_CMD_CHANNEL(vehMobPL_format, chanName.str().c_str(),
+			  THIS_PROCESS, config_file);
+  }
+  if (!vehMobPLCfgBuf->valid()) {
+    delete vehMobPLCfgBuf;
+    vehMobPLCfgBuf = 0;
+    return 1;
+  }
+
+  {
+    ostringstream chanName;
+    chanName << VEH_MOB_PL_SET_NAME << bufnum;
+    vehMobPLSetBuf =
+      new RCS_STAT_CHANNEL(vehMobPL_format, chanName.str().c_str(),
+			   THIS_PROCESS, config_file);
+  }
+  if (!vehMobPLSetBuf->valid()) {
+    delete vehMobPLSetBuf;
+    vehMobPLSetBuf = 0;
+    return 1;
+  }
+  
+  return 0;
+}
+
+template<class C> static inline char* rcs_stat_to_string(const C& s) {
+  switch(s) {
+  case RCS_DONE: return "RCS_DONE";
+  case RCS_EXEC: return "RCS_EXEC";
+  case RCS_ERROR: return "RCS_ERROR";
+  case UNINITIALIZED_STATUS: return "UNINITIALIZED_STATUS";
+  } 
+  return "?";
+}
+
+ostream& operator<<(ostream& out, const PM_CARTESIAN& tran) {
+  return out << "[x=" << tran.x << " y=" << tran.y << " z=" << tran.z << ']';
+}
+
+ostream& operator<<(ostream& out, const PM_ROTATION_MATRIX& rot) {
+  PM_RPY rpy(rot);
+  return out << "[r=" << rpy.r << " p=" << rpy.p << " y=" << rpy.y << ']';
+}
+
+ostream& operator<<(ostream& out, const PM_HOMOGENEOUS& pose) {
+  return out << "[tran=" << pose.tran << " rot=" << pose.rot << ']';
+}
+
+ostream& Robot::printStat(ostream& out) const {
+  return out << "command_type:       " 
+	     << vehMobPL_symbol_lookup(statP->command_type) << endl
+	     << "echo_serial_number: " << statP->echo_serial_number << endl
+	     << "prim serial number: " << primStatP->echo_serial_number << endl
+	     << "status:             " 
+	     << rcs_stat_to_string(statP->status) << endl
+	     << "state:              " << statP->state << endl
+	     << "line:               " << statP->line << endl
+	     << "source_line:        " << statP->source_line << endl
+	     << "source_file:        " << statP->source_file << endl
+	     << "heartbeat:          " << statP->heartbeat << endl
+	     << "set cycleTime:      " << setP->cycleTime << endl
+	     << "actual cycleTime:   " << statP->cycleTime << endl
+	     << "plan cost:          " << statP->planCost << endl
+	     << "debug:              " << setP->debug << endl
+	     << "pose:               " << statP->pose << endl
+	     << "message:            " << statP->message << endl;
+}
+
+/*
+void readImage(const char* file) {
+  ifstream i(file, ios::in|ios::binary|ios::ate);
+  if(!i.is_open()) {
+    cerr << "file: " << file << " not found" << endl;
+    return;
+  }
+  image_size = i.tellg();
+  char *memblock = new char[image_size];
+  i.seekg(0, ios::beg);
+  i.read(memblock, image_size);
+  image = (unsigned char*) memblock;
+}
+*/
+
+float mod(const float& a, const float& b) {
+  int whole_divisions = (int)(a/b);
+  return a - whole_divisions*b;
+}
+
+static float canonical_angle(const float& f) {
+  float retval = mod(f,(2*(float)PI));
+  return retval<0? retval+2*(float)PI: retval;
+}
+
+unsigned int Hash (string const& s)
+{
+  unsigned int result = 0;
+  for (unsigned int i = 0; s [i] != 0; ++i)
+    result = (result & mask) ^ (result << shift) ^ s [i];
+  return result;
+}
+
+float Robot::turn(float goal_r, float r) {
+  float diff = mod((goal_r-r), (2*(float)PI));
+  if(diff > PI) return diff-2*(float)PI;
+  if(diff < -PI) return diff+2*(float)PI;
+  return diff;
+}
+
+void* Robot::simulate(void *thisp) {
+  Robot* me = (Robot*) thisp;
+  
+  sleep(5);
+
+  me->vehMobPLStatBuf->read();
+  me->vehMobPLSetBuf->read();
+  me->primMobJAStatBuf->read();
+
+  print_debugln("command initialization");
+  VehMobPLCmdInit init = VehMobPLCmdInit();
+  init.serial_number = me->statP->echo_serial_number+1;
+  me->vehMobPLCmdBuf->write(&init);
+  //i think primMobJA is initted by vehMobPL
+
+  sleep(5);
+  me->vehMobPLStatBuf->read();
+  me->vehMobPLSetBuf->read();
+  me->primMobJAStatBuf->read();
+
+  me->printStat(cerr);
+  /*
+  print_debugln("try to get image");
+  const unsigned char* image;
+  int image_size;
+  ImageClient *ic = new ImageClient("192.168.1.100", 
+				    (unsigned short)5003, &image_size, image);
+  cerr << "got some kind of image, size: " << image_size << endl;
+  */
+  /*
+  sleep(5);
+
+  me->vehMobPLStatBuf->read();
+  me->vehMobPLSetBuf->read();
+
+  me->printStat(cerr);
+  print_debugln("try to move");
+  VehMobPLCmdMoveWaypoint move = VehMobPLCmdMoveWaypoint();
+  move.p.x = 4;
+  move.p.y = 0;
+  move.serial_number = me->statP->echo_serial_number+1;
+  move.isGlobal = false;
+  move.neighborhood = 1;
+  move.startTime = 0;
+  me->vehMobPLCmdBuf->write(&move);
+  
+  sleep(5);
+  me->vehMobPLStatBuf->read();
+  me->vehMobPLSetBuf->read();
+  me->printStat(cerr);
+  */
+
+  //start dumping the pic
+  //VehMobPLCfgDumpWM dump = VehMobPLCfgDumpWM();
+  //dump.serial_number = me->setP->echo_serial_number + 1;
+  //dump.skip = 5;
+  //dump.dumpContinuous = true;
+  //strcpy(dump.fileName, "../../Agents/PenDecoder/VehMobPLDispOutput.ppm");
+  //strcpy(dump.fileName, "/tmp/VehMobPLDispOutput.ppm");
+  //me->vehMobPLCfgBuf->write(&dump);
+
+  print_debugln("entering for loop");
+  //EnterCriticalSection(&me->CriticalSection);
+  for(int i;;i++) {
+    //LeaveCriticalSection(&me->CriticalSection);
+    do {
+      const Boeing::MsgCmd *bmsg;
+      //if(me->server_->isConnected())  print_debug('.');
+      while(me->server_->isConnected() && 
+	 (bmsg = me->server_->getNextMessage())) {
+	Msg* msg = Msg::interpret(bmsg->buff);
+	//if(!dynamic_cast<MsgReqLocationPP*>(msg)) {
+	//  print_debug("got message: "); print_debugln(msg->render());
+	//}
+	me->callback(msg, NULL);
+      } 
+      sleep(updatePeriod);
+      //} while(!TryEnterCriticalSection(&me->CriticalSection));
+    } while(true);
+    /*
+    if(me->action == SHUTDOWN) break;
+    if(me->action == PAUSED) continue;
+    if(me->action == WAITING) {
+      if(me->goals.empty()) continue;
+      me->setGoal(me->goals.top().first, me->goals.top().second);
+    }
+    if(me->action == GO_TO_GOAL || me->action == FOLLOWING) {
+      //goal is revisited each step when following
+      if(me->action == FOLLOWING) {
+	map<string, Robot*>::const_iterator i = me->robots->find(me->followee);
+	if(i == me->robots->end()) throw exception();
+	me->goal_x = i->second->x;
+	me->goal_y = i->second->y;
+      }
+      
+      //calculate difference between cur_pos and goal
+      float x_vec = me->goal_x-me->x;
+      float y_vec = me->goal_y-me->y;
+      float distance_to_goal = sqrt(x_vec*x_vec+y_vec*y_vec);
+      if(distance_to_goal <= personal_space) {
+	if(me->action == GO_TO_GOAL) {
+	  me->action = WAITING;
+	  cerr << "done" << endl;
+	}
+	continue;
+      }
+      //are we facing the right direction to translate?
+      float mini_goal_r = canonical_angle(atan2(y_vec, x_vec));
+      float r_vec = turn(mini_goal_r, me->r);
+      if(r_vec) {
+	//turn to face the right direction
+	float period_turn = updatePeriod*max_r_velocity;
+	if(abs(r_vec) <= period_turn) me->r = mini_goal_r;			
+	else if(r_vec > 0) {
+	  me->r = canonical_angle(me->r + period_turn);
+	  continue;
+	} else {
+	  me->r = canonical_angle(me->r - period_turn);
+	  continue;
+	}
+      }
+      //move at speed
+      float period_movement = updatePeriod*velocity;
+      me->x += cos(mini_goal_r)*period_movement;
+      me->y += sin(mini_goal_r)*period_movement;
+    } else if(me->action == GO_TO_ANGLE) {
+      float r_vec = turn(me->goal_r, me->r);
+      float period_turn = updatePeriod*max_r_velocity;
+      if(abs(r_vec) <= period_turn) {
+	me->r = me->goal_r;
+	cerr << "done turning" << endl;
+	me->action = WAITING;
+      } else if(r_vec > 0) me->r = canonical_angle(me->r + period_turn); 
+      else me->r = canonical_angle(me->r - period_turn);
+    }
+    */
+  }
+}
+
+float Robot::getX() const {return statP->pose.tran.x;}
+float Robot::getY() const {return statP->pose.tran.y;}
+float Robot::getYaw() const {return PM_RPY(statP->pose.rot).y;}
+
+Robot::Robot(const map<string, Robot*> *robs, 
+	     string name, int simulator_index, unsigned int port)
+  : action(WAITING), sim_index(simulator_index), robots(robs), 
+    imageClient(NULL), vehMobPLCmdBuf(NULL), vehMobPLStatBuf(NULL), 
+    vehMobPLCfgBuf(NULL), vehMobPLSetBuf(NULL)
+{
+  //InitializeCriticalSection(&CriticalSection);
+  sim_init();
+  char nml_file_default[] = DEFAULT_NML_FILE;
+  char *nml_file_env;
+  char *NML_FILE = NULL;
+  char ini_file_default[] = DEFAULT_INI_FILE;
+  char *ini_file_env;
+  char *INI_FILE = NULL;
+
+  // get the config file name from arg, if provided, next environment,
+  // if there, else default
+  if (NULL == NML_FILE) {
+    nml_file_env = getenv("CONFIG_NML");
+    if (NULL != nml_file_env) {
+      NML_FILE = nml_file_env;
+    } else {
+      NML_FILE = nml_file_default;
+    }
+  }
+
+  // ditto for the .ini file
+  if (NULL == INI_FILE) {
+    ini_file_env = getenv("CONFIG_INI");
+    if (NULL != ini_file_env) {
+      INI_FILE = ini_file_env;
+    } else {
+      INI_FILE = ini_file_default;
+    }
+  }
+
+  // initial allocate all the buffers
+  print_debugln("initial allocation of the buffers");
+  if(initVehMobPLNmlBuffers(NML_FILE, sim_index)) {
+    cerr << "can't allocate NML buffers" << endl;
+  }
+
+  // set up our convenient pointer to status and settings
+  print_debugln("set up convenient pointers");
+  statP = (VehMobPLStat *) vehMobPLStatBuf->get_address();
+  primStatP = (PrimMobJAStat *) primMobJAStatBuf->get_address();
+  setP = (VehMobPLSet *) vehMobPLSetBuf->get_address();
+
+  print_debugln("new robotserver");
+  server_ = new Boeing::RobotServer();
+  print_debugln("opening port");
+  if (port) server_->open(port);
+  else server_->open();
+  print_debugln("starting reading thread");
+  pthread_t simulation_thread;
+  pthread_create(&simulation_thread, NULL, simulate, (void *)this);
+  print_debugln("done with robot constructor");
+}
+
+Robot::~Robot() {
+  cleanupVehMobPLNmlBuffers();
+  for(vector<int>::const_iterator i=pids.begin(); i!=pids.end(); i++) {
+    cerr << "killing " << *i << endl;
+    kill(*i, SIGQUIT);
+  }
+  delete server_;
+}
+
+void Robot::callback(const Msg* msgp, const sockaddr* st_remote) {
+  //print_debugln("got a message");
+  vehMobPLStatBuf->read();
+  vehMobPLSetBuf->read();
+  primMobJAStatBuf->read();
+
+  Msg* temp_message;
+  if(const MsgCmdActionPP* action = dynamic_cast<const MsgCmdActionPP*>(msgp)) {
+    //print_debugln("got an action");
+    //need to translate txt-based message into typed message
+    temp_message = Msg::interpretPlayAction(*action->content());
+  } else {
+    temp_message = msgp->clone();
+  }
+  if(const MsgMapReqPP* req_image = dynamic_cast<const MsgMapReqPP*>(temp_message)) {
+    //print_debugln("got an req image");
+    //get the jpeg from the usarsim video socket
+    const unsigned char* image;
+    int image_size = 0;
+    if(imageClient == NULL) {
+      imageClient = new ImageClient("sylvester.speech.cs.cmu.edu", 
+                                    (unsigned short)5333);
+    }
+    //print_debugln("getting the image");
+    image_size = imageClient->getImage(image);
+    //print_debugln("got the image");
+
+    if(image_size > 0) {
+      //send the image to the robot client
+      server_->sendJPEGImage(image, image_size, 320, 240, req_image->getInvoice());
+    } else {
+      cerr << "Image size is 0" << endl;
+    }
+    delete temp_message;
+    //print_debugln("done with req image");
+    return;
+  }
+  if(dynamic_cast<const MsgReqLocationPP*>(temp_message)) {
+    //print_debugln("got an req location");
+    server_->sendLocation(getX(), -getY(), canonical_angle(-getYaw()), true);
+    delete temp_message;
+    return;
+  }
+  //EnterCriticalSection(&CriticalSection);
+  if(const MsgCmdGoToRelativePP* goto_relative = dynamic_cast<const MsgCmdGoToRelativePP*>(temp_message)) {
+    cout << "got: " << *goto_relative << endl;
+    float m_x = goto_relative->content()->x;
+    float m_y = -goto_relative->content()->y;
+    float dir = getYaw()+atan2(m_y, m_x);
+    float len = sqrt(m_x*m_x+m_y*m_y);
+    VehMobPLCmdMoveWaypoint move = VehMobPLCmdMoveWaypoint();
+    move.p.x = getX()+cos(dir)*len;
+    move.p.y = getY()+sin(dir)*len;
+    //cerr << "m_x: " << m_x << " m_y: " << m_y << " yaw: " << getYaw()
+    // << " curdir: " << atan2(m_y, m_x) << " dir: " << dir
+    // << " len: " << len << " c_x: " << getX() << " c_y: " << getY()
+    // << " movex: " << move.p.x << " movey: " << move.p.y << endl;
+    move.serial_number = statP->echo_serial_number+1;
+    move.isGlobal = true;
+    move.neighborhood = 1;
+    move.startTime = 0;
+    vehMobPLCmdBuf->write(&move);
+    delete temp_message;
+    return;
+  } else if(const MsgCmdHaltPP* halt = dynamic_cast<const MsgCmdHaltPP*>(temp_message)) {
+    cout << "got: " << *halt << endl;
+    action = WAITING;
+    delete temp_message;
+    return;
+  } else if(const MsgCmdGoToGlobalPP* goto_global = dynamic_cast<const MsgCmdGoToGlobalPP*>(temp_message)) {
+    cout << "got: " << *goto_global << endl;
+    //setGoal(goto_global->content()->x, goto_global->content()->y);
+    VehMobPLCmdMoveWaypoint move = VehMobPLCmdMoveWaypoint();
+    move.p.x = goto_global->content()->x;
+    move.p.y = -goto_global->content()->y;
+    move.serial_number = statP->echo_serial_number+1;
+    move.isGlobal = true;
+    move.neighborhood = 1;
+    move.startTime = 0;
+    vehMobPLCmdBuf->write(&move);
+    delete temp_message;
+    return;
+  } else if(const MsgCmdTurnRelativePP* turn = dynamic_cast<const MsgCmdTurnRelativePP*>(temp_message)) {
+    cout << "got: " << *turn << endl;
+    PrimMobJACmdRotate rotate = PrimMobJACmdRotate();
+    float r = -getYaw();
+    rotate.theta = canonical_angle(-(r+turn->content()->angle));
+    rotate.thetaTol = PI/20; //9 degrees tolerance
+    // always make it a new command
+    rotate.serial_number = primStatP->echo_serial_number + 1;
+    primMobJACmdBuf->write(&rotate);
+    //goal_r = canonical_angle(r+turn->content()->angle);
+    //action = GO_TO_ANGLE; //will report done when complete
+    delete temp_message;
+    return;
+  } else if(const MsgCmdHomePP* home = dynamic_cast<const MsgCmdHomePP*>(temp_message)) {
+    cout << "got: " << *home << endl;
+    VehMobPLCmdMoveWaypoint move = VehMobPLCmdMoveWaypoint();
+    move.p.x = 0;
+    move.p.y = 0;
+    move.serial_number = statP->echo_serial_number+1;
+    move.isGlobal = true;
+    move.neighborhood = 1;
+    move.startTime = 0;
+    vehMobPLCmdBuf->write(&move);
+    delete temp_message;
+    return;
+  } else if(const MsgCmdPausePP* pause = dynamic_cast<const MsgCmdPausePP*>(temp_message)) {
+    cout << "got: " << *pause << endl;
+    if(action != PAUSED) {
+      paused_action = action;
+      action = PAUSED;
+    }
+  } else if(const MsgCmdResumePP* resume = dynamic_cast<const MsgCmdResumePP*>(temp_message)) {
+    cout << "got: " << *resume << endl;
+    if(action == PAUSED) {
+      action = paused_action;
+    }
+  } else if(const MsgCmdFollowPP* follow = dynamic_cast<const MsgCmdFollowPP*>(temp_message)) {
+    cout << "got: " << *follow << endl;
+    //setFollow(follow->content()->leader);
+  } 
+  //LeaveCriticalSection(&CriticalSection);
+  delete temp_message;
+}
+
+/**
+ * Startup the moast porcesses that this robot requires
+ */
+void Robot::sim_init() {
+  vector<string> args;
+  {
+    ostringstream arg;
+    arg << "-i" << sim_index;
+    args.push_back(arg.str());
+  }
+  if(sim_index != 1) {
+    ostringstream arg;
+    arg << "-s" << sim_index;
+    args.push_back(arg.str());
+  }
+  //args.push_back("-v");
+  spawn(false, MOAST_BIN, "usarSim", args);
+  sleep(60);
+
+  args.clear();
+  {
+    ostringstream arg;
+    arg << "-d" << sim_index;
+    args.push_back(arg.str());
+  }
+  if(sim_index==1) sleep(2);
+  pids.push_back(spawn(false, MOAST_BIN, "primMobMain", args)); 
+  pids.push_back(spawn(false, MOAST_BIN, "primSPMain", args));
+  if(sim_index==1) pids.push_back(spawn(false, MOAST_BIN, "primMisMain", args));
+  sleep(2);
+  pids.push_back(spawn(false, MOAST_BIN, "amMobMain", args));
+
+  args.clear();
+  args.push_back("-z.1");
+  {
+    ostringstream arg;
+    arg << "-d" << sim_index;
+    args.push_back(arg.str());
+  }
+  pids.push_back(spawn(false, MOAST_BIN, "amSPMain", args));
+
+  args.clear();
+  args.push_back("-i");
+  args.push_back("-g3");
+  args.push_back("-r.2");
+  {
+    ostringstream arg;
+    arg << "-d" << sim_index;
+    args.push_back(arg.str());
+  }
+  pids.push_back(spawn(false, MOAST_BIN, "vehMobPLMain", args));
+  sleep(6);
+}
+
+//HANDLE Robot::getSimulationHandle() const {return simulationThread_;}
+
+//void Robot::wait() const {WaitForSingleObject(simulationThread_, INFINITE);}
+
+//some actual actions
+void Robot::setGoal(float x, float y) {
+  //goal_x = x;
+  //goal_y = y;
+  //action = GO_TO_GOAL;
+}
+
+void Robot::stackGoal(float x, float y) {
+  //goals.push(pair<float, float>(x, y));
+}
+
+void Robot::setFollow(const string& rname) {
+  followee = rname;
+  action = FOLLOWING; //follow is never complete
+}


Property changes on: TeamTalk/Agents/TeamTalkSimulator/robot.cc
___________________________________________________________________
Name: svn:executable
   + 

Added: TeamTalk/Agents/TeamTalkSimulator/robot.h
===================================================================
--- TeamTalk/Agents/TeamTalkSimulator/robot.h	                        (rev 0)
+++ TeamTalk/Agents/TeamTalkSimulator/robot.h	2006-12-11 03:46:02 UTC (rev 555)
@@ -0,0 +1,78 @@
+#ifndef ROBOT_H
+#define ROBOT_H
+
+#include <boeing_robot_server.h>
+#include <robot_packet.hpp>
+
+#include <map>
+#include <stack>
+
+#include <rcs.hh>
+#include <moastTypes.hh>	// MOAST_NML_BUFFER_NAME_LEN
+#include <moastNmlOffsets.hh>	// NAV_DATA_BASE, etc.
+#include <vehMobPL.hh>		// vehMobPL_format()
+#include <primMobJA.hh>         // primMobJA_format()
+
+#include "imageClient.h"
+
+using namespace std;
+
+//class Robot : public CallBackClass {
+class Robot {
+protected:
+  //CRITICAL_SECTION CriticalSection;
+  typedef enum {WAITING=0, ACTING, PAUSED, FOLLOWING, SHUTDOWN} action_t;
+  action_t action, paused_action;
+  int sim_index;
+  string followee;
+  const map<string, Robot*> *robots;
+  vector<int> pids; //keep pids for later destruction
+  
+  //HANDLE simulationThread_;
+  Boeing::RobotServer *server_;
+  ImageClient *imageClient;
+  
+  static float turn(float goal_r, float r);
+  
+  //moast/rcs structures
+  RCS_CMD_CHANNEL *vehMobPLCmdBuf;
+  RCS_STAT_CHANNEL *vehMobPLStatBuf;
+  RCS_CMD_CHANNEL *primMobJACmdBuf;
+  RCS_STAT_CHANNEL *primMobJAStatBuf;
+  RCS_CMD_CHANNEL *vehMobPLCfgBuf;
+  RCS_STAT_CHANNEL *vehMobPLSetBuf;
+  VehMobPLStat *statP;
+  VehMobPLSet *setP;
+  PrimMobJAStat *primStatP;
+
+  int cleanupVehMobPLNmlBuffers();
+  int initVehMobPLNmlBuffers(char *config, int bufnum);
+  static const int updatePeriod = 1;
+  static void* simulate(void *thisp);
+  
+  //these are protected because the mechanism requires synchrnization
+  //with an nml buffer read
+  float getX() const; //current X-coord in globalpos frame in meters
+  float getY() const; //current Y-ccord in globalpos frame in meters
+  float getYaw() const; //current yaw in globalpos frame in radians
+
+ public:
+  Robot(const map<string, Robot*>* robots, 
+	string name, int simulator_index, unsigned int port=0);
+  ~Robot();
+  void callback(const Msg* msgp, const sockaddr* st_remote);
+  //HANDLE getSimulationHandle() const;
+  //void wait() const;
+  void sim_init();
+
+  //getters
+  Boeing::RobotServer *getServer();
+  ostream& printStat(ostream& out) const;
+
+  //some actual commands
+  void setGoal(float x, float y);
+  void stackGoal(float x, float y);
+  void setFollow(const string& rname);
+};
+
+#endif


Property changes on: TeamTalk/Agents/TeamTalkSimulator/robot.h
___________________________________________________________________
Name: svn:executable
   + 

Added: TeamTalk/Agents/TeamTalkSimulator/runSimulator.sh
===================================================================
--- TeamTalk/Agents/TeamTalkSimulator/runSimulator.sh	                        (rev 0)
+++ TeamTalk/Agents/TeamTalkSimulator/runSimulator.sh	2006-12-11 03:46:02 UTC (rev 555)
@@ -0,0 +1,130 @@
+#!/bin/bash
+
+echo "This job was submitted by user: $PBS_O_LOGNAME"
+echo "This job was submitted to host: $PBS_O_HOST"
+echo "This job was submitted to queue: $PBS_O_QUEUE"
+echo "PBS working directory: $PBS_O_WORKDIR"
+echo "PBS job id: $PBS_JOBID"
+echo "PBS job name: $PBS_JOBNAME"
+echo "PBS environment: $PBS_ENVIRONMENT"
+echo " "
+echo "This script is running on `hostname` "
+
+export CONFIG_NML=/net/eucalyptus/usr0/tkharris/moast-1.2/etc/moast.nml
+export CONFIG_INI=/net/eucalyptus/usr0/tkharris/moast-1.2/etc/moast.ini
+./TeamTalkSimulator --peerfile peerfile.txt --mapserver
+
+# cd ../../../moast-1.2/bin
+# HOME=/net/eucalyptus/usr0/tkharris
+# MOAST=$HOME/moast-1.2
+# export RCSLIB_DIR=$HOME/lib/rcslib
+
+# echo `dirname $0`/..
+
+# #set SECT=yes to run section, VEH, AM, prim, and servo, else SECT=no
+# SECT=no
+
+# # if SECT=no, set VEH=yes to run vehicle, AM, prim, and servo, else VEH=no
+# VEH=yes
+
+# # if VEH=no, set AM=yes to run AM, prim, and servo, else AM=no
+# AM=yes
+
+# # if AM=no, set PRIM=yes to run prim, and servo, else PRIM=no
+# PRIM=yes
+
+# # set USARSIM=yes to run usarSim, else USARSIM=no to run simpleSim
+# USARSIM=yes
+
+# CONFIG_NML=$MOAST/etc/moast.nml ; export CONFIG_NML
+# CONFIG_INI=$MOAST/etc/moast.ini ; export CONFIG_INI
+
+# TOP=servoShell
+# if [ x$PRIM = xyes ] ; then TOP=primShell ; fi
+# if [ x$AM = xyes ] ; then PRIM=yes; TOP=amShell ; fi
+# if [ x$VEH = xyes ] ; then AM=yes; PRIM=yes; TOP=vehShell ; fi
+# if [ x$SECT = xyes ] ; then VEH=yes; AM=yes; PRIM=yes; TOP=sectShell ; fi
+# pid1=0
+# pid2=0
+# pid3=0
+# pid4=0
+# pid5=0
+# pid6=0
+# pid7=0
+# pid8=0
+# pid9=0
+
+# kill1=9
+# kill2=9
+# kill3=9
+# kill4=9
+# kill5=9
+# kill6=INT
+# kill7=9
+# kill8=9
+# kill9=9
+
+# ./ipc-clear
+
+# #./splash $CONFIG_INI &
+
+# echo "Starting server..."
+# ./moastNmlSvr &
+# sleep 2
+# pid1=$!
+
+# if [ x$USARSIM = xyes ] ; then
+#   echo "Starting usarsim..."
+#   ./usarSim -i1 &
+#   sleep 30
+#   kill2=HUP
+# else
+#   ./simpleSim &
+# fi
+# pid2=$!
+
+# if [ x$PRIM = xyes ] ; then
+#   echo "Starting prim..."
+#   sleep 2
+#   ./primMobMain &
+#   pid3=$!
+#   ./primSPMain &
+#   pid4=$!
+#   ./primMisMain &
+#   pid5=$!
+# fi
+
+# if [ x$AM = xyes ] ; then
+#     echo "Starting am..."
+#   ./amMobMain &
+#   pid6=$!
+#   ./amSPMain -z.1&
+#   pid7=$!
+# fi
+
+# if [ x$VEH = xyes ] ; then
+#   echo "Starting vehicle..."
+#   ./vehMobPLMain -i -g3 -r.2&
+#   pid8=$!
+# fi
+
+# if [ x$SECT = xyes ] ; then
+#   echo "Starting section..."
+#   ./sectMobPL &
+#   pid9=$!
+# fi
+
+# #./$TOP
+# sleep 60
+
+# if [ ! $pid9 = 0 ] ; then kill -$kill9 $pid9 ; fi
+# if [ ! $pid8 = 0 ] ; then kill -$kill8 $pid8 ; fi
+# if [ ! $pid7 = 0 ] ; then kill -$kill7 $pid7 ; fi
+# if [ ! $pid6 = 0 ] ; then kill -$kill6 $pid6 ; fi
+# if [ ! $pid5 = 0 ] ; then kill -$kill5 $pid5 ; fi
+# if [ ! $pid4 = 0 ] ; then kill -$kill4 $pid4 ; fi
+# if [ ! $pid3 = 0 ] ; then kill -$kill3 $pid3 ; fi
+# if [ ! $pid2 = 0 ] ; then kill -$kill2 $pid2 ; fi
+# if [ ! $pid1 = 0 ] ; then kill -$kill1 $pid1 ; fi
+
+# exit 0


Property changes on: TeamTalk/Agents/TeamTalkSimulator/runSimulator.sh
___________________________________________________________________
Name: svn:executable
   +