[TeamTalk 345]: [881] trunk/moast-bth: 1) New haness for running moast on multiple machines .
tk@edam.speech.cs.cmu.edu
tk at edam.speech.cs.cmu.edu
Tue Dec 4 16:03:07 EST 2007
An HTML attachment was scrubbed...
URL: http://mailman.srv.cs.cmu.edu/pipermail/teamtalk-developers/attachments/20071204/5c495697/attachment-0001.html
-------------- next part --------------
Modified: trunk/moast-bth/TeamTalkSimulator/Makefile
===================================================================
--- trunk/moast-bth/TeamTalkSimulator/Makefile 2007-12-04 20:52:26 UTC (rev 880)
+++ trunk/moast-bth/TeamTalkSimulator/Makefile 2007-12-04 21:03:07 UTC (rev 881)
@@ -1,67 +1,71 @@
-MOASTLIB_DIR = ../moast
-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
-
-VERBOSE = 1
-
-DEP_ECHO = @echo Dependency: $@
-CC_ECHO = @echo Compiling: $@
-LINK_ECHO = @echo Linking: $@
-CLEAN_ECHO = @echo Cleaning.
-
-ifneq ($(VERBOSE), 1)
- Q := @
-endif
-
-CPPFLAGS = $(DEFS) $(INCLUDES)
-CXXFLAGS = -g -Wall -O0
-CXXCOMPILE = $(CXX) -c $(CPPFLAGS) $(CXXFLAGS)
-CXXLINK = $(CXX) $(CXXFLAGS) $(LDFLAGS) -o $@
-
-OBJECTS = TeamTalkSimulator.o robot.o imageClient.o
-
-EXECUTABLES = TeamTalkSimulator
-
-all: $(EXECUTABLES)
-
-# Include the dependencies
--include $(OBJECTS:.o=.d)
-
-TeamTalkSimulator: $(PRIMITIVECOMM_DIR)/libboeingrobotserver.a $(OBJECTS)
- @rm -f TeamTalkSimulatedBot
- $(CXXLINK) $(OBJECTS) $(PRIMITIVECOMM_DIR)/libboeingrobotserver.a $(LDLIBS)
-
-$(PRIMITIVECOMM_DIR)/libboeingrobotserver.a: force
- $(MAKE) -C $(PRIMITIVECOMM_DIR) all
-
-# Compilation rule
-%.o: %.cc
- $(CC_ECHO)
- $(Q)$(CXXCOMPILE) -o $@ $<
-
-%.o: %.cpp
- $(CC_ECHO)
- $(Q)$(CXXCOMPILE) -o $@ $<
-
-# Dependency generation rule
-%.d: %.cc
- $(DEP_ECHO)
- $(Q)$(CXX) -c $(CPPFLAGS) -MM $< | ../fixdepend $(dir $<) > $@
-
-%.d: %.cpp
- $(DEP_ECHO)
- $(Q)$(CXX) -c $(CPPFLAGS) -MM $< | ../fixdepend $(dir $<) > $@
-
-# Clean rule
-clean:
- $(MAKE) -C $(PRIMITIVECOMM_DIR) clean
- rm -f *.o *.d $(EXECUTABLES)
-
+MOASTLIB_DIR = ../moast
+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
+
+VERBOSE = 1
+
+DEP_ECHO = @echo Dependency: $@
+CC_ECHO = @echo Compiling: $@
+LINK_ECHO = @echo Linking: $@
+CLEAN_ECHO = @echo Cleaning.
+
+ifneq ($(VERBOSE), 1)
+ Q := @
+endif
+
+CPPFLAGS = $(DEFS) $(INCLUDES)
+CXXFLAGS = -g -Wall -O0
+CXXCOMPILE = $(CXX) -c $(CPPFLAGS) $(CXXFLAGS)
+CXXLINK = $(CXX) $(CXXFLAGS) $(LDFLAGS) -o $@
+
+TTSIMOBJECTS = TeamTalkSimulator.o robot.o imageClient.o
+ROBOSIMOBJECTS = robotSimulator.o robot.o imageClient.o
+OBJECTS = $(TTSIMOBJECTS) $(ROBOSIMOBJECTS)
+
+EXECUTABLES = TeamTalkSimulator robotSimulator
+
+all: $(EXECUTABLES)
+
+# Include the dependencies
+-include $(OBJECTS:.o=.d)
+
+TeamTalkSimulator: $(PRIMITIVECOMM_DIR)/libboeingrobotserver.a $(TTSIMOBJECTS)
+ $(CXXLINK) $(TTSIMOBJECTS) $(PRIMITIVECOMM_DIR)/libboeingrobotserver.a $(LDLIBS)
+
+robotSimulator: $(PRIMITIVECOMM_DIR)/libboeingrobotserver.a $(ROBOSIMOBJECTS)
+ $(CXXLINK) $(ROBOSIMOBJECTS) $(PRIMITIVECOMM_DIR)/libboeingrobotserver.a $(LDLIBS)
+
+$(PRIMITIVECOMM_DIR)/libboeingrobotserver.a: force
+ $(MAKE) -C $(PRIMITIVECOMM_DIR) all
+
+# Compilation rule
+%.o: %.cc
+ $(CC_ECHO)
+ $(Q)$(CXXCOMPILE) -o $@ $<
+
+%.o: %.cpp
+ $(CC_ECHO)
+ $(Q)$(CXXCOMPILE) -o $@ $<
+
+# Dependency generation rule
+%.d: %.cc
+ $(DEP_ECHO)
+ $(Q)$(CXX) -c $(CPPFLAGS) -MM $< | ../fixdepend $(dir $<) > $@
+
+%.d: %.cpp
+ $(DEP_ECHO)
+ $(Q)$(CXX) -c $(CPPFLAGS) -MM $< | ../fixdepend $(dir $<) > $@
+
+# Clean rule
+clean:
+ $(MAKE) -C $(PRIMITIVECOMM_DIR) clean
+ rm -f *.o *.d $(EXECUTABLES)
+
force: ;
\ No newline at end of file
Added: trunk/moast-bth/TeamTalkSimulator/Simulator.pl
===================================================================
--- trunk/moast-bth/TeamTalkSimulator/Simulator.pl (rev 0)
+++ trunk/moast-bth/TeamTalkSimulator/Simulator.pl 2007-12-04 21:03:07 UTC (rev 881)
@@ -0,0 +1,103 @@
+#!/usr/bin/perl
+
+use strict;
+use File::Spec;
+
+require 'util.pl';
+
+my $MOAST_BTH=File::Spec->rel2abs(File::Spec->updir);
+my $BOTSCRIPT=File::Spec->rel2abs('runBot.pl');
+
+#remove old log files
+unlink glob "$MOAST_BTH/log/*";
+
+my @robots = &get_bots('peerfile.txt');
+$SIG{QUIT} = $SIG{INT} = $SIG{TERM} = \&signal_children;
+my $num_bots = scalar @robots;
+
+print $/;
+print "ticket: $ENV{'PBS_JOBID'}$/";
+
+my %ini;
+&readINI('Simulator.ini', \%ini);
+
+my $hoststring;
+open(STATUS, "qstat -f $ENV{'PBS_JOBID'}|");
+$hoststring = (split(/ = /,(grep(/exec_host =/, <STATUS>))[0]))[1];
+chomp $hoststring;
+my @hosts = split(/\+/, $hoststring);
+die "Needed $num_bots" if(scalar @hosts != $num_bots);
+foreach(0..$#robots) {
+ my $host = shift @hosts;
+ $host =~ s|/.*||;
+ ${$robots[$_]}{'host'} = $host;
+}
+
+foreach(@robots) {
+ my $cmd = "ssh ${$_}{'host'} $BOTSCRIPT --moast-bth=$MOAST_BTH --id=${$_}{'id'} --type=${$_}{'type'} --name=${$_}{'name'}";
+ $cmd .= " --display=$ini{'DISPLAY'}" if $defined $ini{'DISPLAY'};
+ print $cmd, $/;
+ ${$_}{'pid'} = &spawn($cmd);
+ ${$_}{'remotepid'} = &getremotepid($_); #required due to ssh bug
+}
+
+print &render_bots(@robots);
+
+#wait for them to all exit
+my $done;
+do {
+ $done = 1;
+ my $kid = waitpid(-1, 0);
+ last if $kid == -1;
+ foreach(@robots) {
+ my $pid = ${$_}{'pid'};
+ if($pid) {
+ if($kid == $pid) {
+ ${$_}{'pid'} = 0;
+ } else {
+ $done = 0;
+ }
+ }
+ }
+} while !$done;
+
+exit;
+
+sub signal_children {
+ my $signal = shift;
+ &killremotes; #required because of ssh bug
+ #otherwise we would just kill our children as in the code below
+ #my @active_children = grep($_, map(${$_}{'pid'}, @robots));
+ #kill $signal, @active_children if scalar @active_children;
+}
+
+#we need this because ssh doesn't pass signals when there's no tty
+#it should be fixed in the next version of openssh
+#see https://bugzilla.mindrot.org/show_bug.cgi?id=396
+sub getremotepid {
+ my $robot = shift;
+ my $logfile = "$MOAST_BTH/log/${$robot}{'name'}.out";
+ my $remotepid;
+ do {
+ sleep 1;
+ print "looking$/";
+ next if !-e $logfile;
+ print "found$/";
+ next if !open(LOG, $logfile);
+ chop($remotepid = <LOG>);
+ print "got: $remotepid$/";
+ } until defined $remotepid;
+ close LOG;
+ return $remotepid;
+}
+
+sub killremotes {
+ foreach(@robots) {
+ my $remotepid = ${$_}{'remotepid'};
+ next if !$remotepid;
+ my $host = ${$_}{'host'};
+ next if !$host;
+ system("ssh $host kill $remotepid");
+ }
+}
+
Property changes on: trunk/moast-bth/TeamTalkSimulator/Simulator.pl
___________________________________________________________________
Name: svn:executable
+ *
Modified: trunk/moast-bth/TeamTalkSimulator/TeamTalkSimulator.cc
===================================================================
--- trunk/moast-bth/TeamTalkSimulator/TeamTalkSimulator.cc 2007-12-04 20:52:26 UTC (rev 880)
+++ trunk/moast-bth/TeamTalkSimulator/TeamTalkSimulator.cc 2007-12-04 21:03:07 UTC (rev 881)
@@ -1,474 +1,468 @@
-/** 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>
-#include <unistd.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;
-
-#ifndef DEFAULT_NML_FILE
-#define DEFAULT_NML_FILE "moast.nml"
-#endif
-#ifndef DEFAULT_INI_FILE
-#define DEFAULT_INI_FILE "moast.ini"
-#endif
-
-#define THIS_PROCESS "sectShell"
-static string MAP_DUMP;
-
-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;
-
-string moast_bin;
-
-/****************** MAP STUFF *********************/
-
-static unsigned char* conveyedMap;
-static unsigned char* currentMap = (unsigned char*)malloc(500000);
-static unsigned char* diffMap;
-static int mapWidth, mapHeight;
-static int mapXOffset, mapYOffset;
-static Boeing::MsgMap* msgMap = (Boeing::MsgMap*)malloc(500000);
-
-void readMapFile() {
- //we're going to do this a lot, so just open once
-
-}
-
-void sendFullMap(Boeing::MapServer *mserver) {
- readMapFile();
- Boeing::MsgMap::MsgMapFactory(Boeing::MAP_X_MAJOR,
- msgMap, currentMap, mapWidth*mapHeight,
- 0, 0, mapWidth, mapHeight);
- //debug << "raw: " << msgMap->map[0]
- // << " rl: " << ((msgMap->map[0]&0xFFFFFF00)>>8)
- // << " rv: " << (msgMap->map[0]&0x000000FF) << endl;
- mserver->sendFullMap(msgMap);
-}
-
-void sendDiffMap(Boeing::MapServer *mserver) {
- readMapFile();
- Boeing::MsgMap::MsgMapFactory(Boeing::MAP_X_MAJOR,
- msgMap, diffMap, mapWidth*mapHeight,
- 0, 0, mapWidth, mapHeight);
- //debug << "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(mserver);
- subscribed = true;
- break;
- case Boeing::MAP_KEEPALIVE:
- break;
- case Boeing::MAP_ACK:
- break;
- case Boeing::MAP_UNSUBSCRIBE:
- subscribed = false;
- break;
- default:
- error << "unknown or unhandled msgtype: " << msg->hdr.type << endl;
- }
- } else {
- if(subscribed)
- //sendDiffMap(mserver);
- sendFullMap(mserver);
- }
- sleep(updatePeriod);
- }
-}
-
-/*************************** END MAP STUFF ****************************/
-
-void* readTrades(void *t) {
- Boeing::TraderServer *tserver = (Boeing::TraderServer *) t;
- for(;;) {
- const Boeing::MsgTraderServer *msg = tserver->getNextMessage();
- if(!msg) {
- //debug << "getNextMessage is NULL" << endl;
- sleep(1000);
- continue;
- }
-
- sectMobPLStatBuf->read();
- sectMobPLSetBuf->read();
-
- if(msg->hdr.type != Boeing::TRADER_TASK) {
- error << "don't know what to do with header type " << msg->hdr.type << endl;
- continue;
- }
- debug << "got: " << msg->msg_task.task << endl;
- istringstream task(msg->msg_task.task);
- string token;
- task >> token;
- if(token != "search") {
- error << "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(MAP_DUMP.c_str());
- if(!f) {
- error << "<TeamTalkSimulator::readMapData> can't open map" << endl;
- return;
- }
- string token;
- if((f >> token).fail() || token != "P6") {
- error << "<TeamTalkSimulator::readMapData> expected P6: " << token << endl;
- return;
- }
- //int oldMapWidth = mapWidth;
- //int oldMapHeight = mapHeight;
- if((f >> mapWidth >> mapHeight).fail() || mapWidth <= 0 || mapHeight <= 0) {
- error << "<TeamTalkSimulator::readMapData> expected mapWidth: "
- << mapWidth << " mapHeight: " << mapHeight << endl;
- return;
- }
- if((f >> token).fail() || token != "255") {
- error << "<TeamTalkSimulator::readMapData> expected 255: " << token << endl;
- return;
- }
- ignoreToEndOfLine(f);
- for(int j=0; j<mapHeight; j++) {
- for(int i=0; i<mapWidth; i++) {
- char r, g, b;
- r = f.get();
- g = f.get();
- b = f.get();
- if(f.fail()) {
- error << "<TeamTalkSimulator::readMapData> failed to read at (" << i << ' ' << j << ')' << endl;
- return;
- }
- if(r == '\0' && g == '\0' && b == '\0') {
- //unknown
- currentMap[j*mapWidth+i] = 0x40;
- } else if(r == '\255' && g == '\0' && b == '\0') {
- //obstacle
- currentMap[j*mapWidth+i] = 0xff;
- } else if(r == '\255' && g == '\255' && b == '\255') {
- //unknown
- currentMap[j*mapWidth+i] = 0x00;
- } else {
- error << "<TeamTalkSimulator::readMapData> unknown encoding at (" << i << ' ' << j << "): (" << (unsigned int)r << ' ' << (unsigned int)g << ' ' << (unsigned int)b << ')' << endl;
- return;
- }
- }
- }
-}
-
-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;
- MAP_DUMP.copy(dump.fileName, MOAST_FILE_NAME_LEN);
- 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) {
- fatal << "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();
- sectInit.subordinate = VEHICLE;
- for(int i=0; i<sectInit.vehList_length; i++) {
- sectInit.vehList[i] = i+1;
- }
- sectMobPLCmdBuf->write(§Init);
-
- return 0;
-}
-
-int main(int argc, char *argv[])
-{
- DebugStream::threashold_ = DebugStream::D; //give me all debug messages
-
- //register sigint handler
- signal(SIGINT, sigint_handler);
-
- {
- ostringstream o;
- o << "/tmp/" << getuid() << "-SectMobPLDispOutput.ppm";
- MAP_DUMP = o.str();
- }
-
- 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 if(!strcmp(argv[i], "--moast"))
- moast_bin = string(argv[++i]) + "/bin";
- else {
- warn << "I don't understand arg: " << argv[i] << endl;
- }
- }
-
- if(moast_bin.empty()) {
- fatal << "please specify the moast directory" << endl;
- exit(1);
- }
-
- //clear ipc and start the moast nml server
- spawn(true, moast_bin, "ipc-clear");
- vector<string> args;
- args.push_back("-s");
- NmlSvrPid = spawn(false, moast_bin, "moastNmlSvr", args);
- sleep(3);
-
- if(!peerfile.empty()) {
- int robot_count = 1;
- debug << "adding robots from " << peerfile << endl;
- ifstream Frobotips(peerfile.c_str());
- if(!Frobotips) {
- error << "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);
- debug << "added " << rname << '@' << port << endl;
- } else {
- robots[rname] = new Robot(&robots, rname, robot_count++);
- debug << "added " << rname << endl;
- }
- ignoreToEndOfLine(Frobotips);
- }
- }
- if(robots.empty()) robots[name] = new Robot(&robots, name, 1);
-
- //initialize section control
- args.clear();
- args.push_back("-r.2");
- SectMobPLPid = spawn(false, moast_bin, "sectMobPL", args);
- sleep(5);
- initSectionControl();
- 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
- info << "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;
-
-}
+/** 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>
+#include <unistd.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;
+
+#ifndef DEFAULT_NML_FILE
+#define DEFAULT_NML_FILE "moast.nml"
+#endif
+#ifndef DEFAULT_INI_FILE
+#define DEFAULT_INI_FILE "moast.ini"
+#endif
+
+#define THIS_PROCESS "sectShell"
+static string MAP_DUMP;
+
+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;
+
+string moast_bin;
+
+/****************** MAP STUFF *********************/
+
+static unsigned char* conveyedMap;
+static unsigned char* currentMap = (unsigned char*)malloc(500000);
+static unsigned char* diffMap;
+static int mapWidth, mapHeight;
+static int mapXOffset, mapYOffset;
+static Boeing::MsgMap* msgMap = (Boeing::MsgMap*)malloc(500000);
+
+void readMapFile() {
+ //we're going to do this a lot, so just open once
+
+}
+
+void sendFullMap(Boeing::MapServer *mserver) {
+ readMapFile();
+ Boeing::MsgMap::MsgMapFactory(Boeing::MAP_X_MAJOR,
+ msgMap, currentMap, mapWidth*mapHeight,
+ 0, 0, mapWidth, mapHeight);
+ mserver->sendFullMap(msgMap);
+}
+
+void sendDiffMap(Boeing::MapServer *mserver) {
+ readMapFile();
+ Boeing::MsgMap::MsgMapFactory(Boeing::MAP_X_MAJOR,
+ msgMap, diffMap, mapWidth*mapHeight,
+ 0, 0, mapWidth, mapHeight);
+ 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(mserver);
+ subscribed = true;
+ break;
+ case Boeing::MAP_KEEPALIVE:
+ break;
+ case Boeing::MAP_ACK:
+ break;
+ case Boeing::MAP_UNSUBSCRIBE:
+ subscribed = false;
+ break;
+ default:
+ error << "unknown or unhandled msgtype: " << msg->hdr.type << endl;
+ }
+ } else {
+ if(subscribed)
+ //sendDiffMap(mserver);
+ sendFullMap(mserver);
+ }
+ sleep(updatePeriod);
+ }
+}
+
+/*************************** END MAP STUFF ****************************/
+
+void* readTrades(void *t) {
+ Boeing::TraderServer *tserver = (Boeing::TraderServer *) t;
+ for(;;) {
+ const Boeing::MsgTraderServer *msg = tserver->getNextMessage();
+ if(!msg) {
+ //debug << "getNextMessage is NULL" << endl;
+ sleep(1000);
+ continue;
+ }
+
+ sectMobPLStatBuf->read();
+ sectMobPLSetBuf->read();
+
+ if(msg->hdr.type != Boeing::TRADER_TASK) {
+ error << "don't know what to do with header type " << msg->hdr.type << endl;
+ continue;
+ }
+ debug << "got: " << msg->msg_task.task << endl;
+ istringstream task(msg->msg_task.task);
+ string token;
+ task >> token;
+ if(token != "search") {
+ error << "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(MAP_DUMP.c_str());
+ if(!f) {
+ error << "<TeamTalkSimulator::readMapData> can't open map" << endl;
+ return;
+ }
+ string token;
+ if((f >> token).fail() || token != "P6") {
+ error << "<TeamTalkSimulator::readMapData> expected P6: " << token << endl;
+ return;
+ }
+ //int oldMapWidth = mapWidth;
+ //int oldMapHeight = mapHeight;
+ if((f >> mapWidth >> mapHeight).fail() || mapWidth <= 0 || mapHeight <= 0) {
+ error << "<TeamTalkSimulator::readMapData> expected mapWidth: "
+ << mapWidth << " mapHeight: " << mapHeight << endl;
+ return;
+ }
+ if((f >> token).fail() || token != "255") {
+ error << "<TeamTalkSimulator::readMapData> expected 255: " << token << endl;
+ return;
+ }
+ ignoreToEndOfLine(f);
+ for(int j=0; j<mapHeight; j++) {
+ for(int i=0; i<mapWidth; i++) {
+ char r, g, b;
+ r = f.get();
+ g = f.get();
+ b = f.get();
+ if(f.fail()) {
+ error << "<TeamTalkSimulator::readMapData> failed to read at (" << i << ' ' << j << ')' << endl;
+ return;
+ }
+ if(r == '\0' && g == '\0' && b == '\0') {
+ //unknown
+ currentMap[j*mapWidth+i] = 0x40;
+ } else if(r == '\255' && g == '\0' && b == '\0') {
+ //obstacle
+ currentMap[j*mapWidth+i] = 0xff;
+ } else if(r == '\255' && g == '\255' && b == '\255') {
+ //unknown
+ currentMap[j*mapWidth+i] = 0x00;
+ } else {
+ error << "<TeamTalkSimulator::readMapData> unknown encoding at (" << i << ' ' << j << "): (" << (unsigned int)r << ' ' << (unsigned int)g << ' ' << (unsigned int)b << ')' << endl;
+ return;
+ }
+ }
+ }
+}
+
+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;
+ MAP_DUMP.copy(dump.fileName, MOAST_FILE_NAME_LEN);
+ 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) {
+ fatal << "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();
+ sectInit.subordinate = VEHICLE;
+ for(int i=0; i<sectInit.vehList_length; i++) {
+ sectInit.vehList[i] = i+1;
+ }
+ sectMobPLCmdBuf->write(§Init);
+
+ return 0;
+}
+
+int main(int argc, char *argv[])
+{
+ DebugStream::threashold_ = DebugStream::D; //give me all debug messages
+
+ //register sigint handler
+ signal(SIGINT, sigint_handler);
+
+ {
+ ostringstream o;
+ o << "/tmp/" << getuid() << "-SectMobPLDispOutput.ppm";
+ MAP_DUMP = o.str();
+ }
+
+ 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 if(!strcmp(argv[i], "--moast"))
+ moast_bin = string(argv[++i]) + "/bin";
+ else {
+ warn << "I don't understand arg: " << argv[i] << endl;
+ }
+ }
+
+ if(moast_bin.empty()) {
+ fatal << "please specify the moast directory" << endl;
+ exit(1);
+ }
+
+ //clear ipc and start the moast nml server
+ spawn(true, moast_bin, "ipc-clear");
+ vector<string> args;
+ args.push_back("-s");
+ NmlSvrPid = spawn(false, moast_bin, "moastNmlSvr", args);
+ sleep(3);
+
+ if(!peerfile.empty()) {
+ int robot_count = 1;
+ debug << "adding robots from " << peerfile << endl;
+ ifstream Frobotips(peerfile.c_str());
+ if(!Frobotips) {
+ error << "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(robot_count++, port);
+ debug << "added " << rname << '@' << port << endl;
+ } else {
+ robots[rname] = new Robot(robot_count++);
+ debug << "added " << rname << endl;
+ }
+ ignoreToEndOfLine(Frobotips);
+ }
+ }
+ if(robots.empty()) robots[name] = new Robot(1);
+
+ //initialize section control
+ args.clear();
+ args.push_back("-r.2");
+ SectMobPLPid = spawn(false, moast_bin, "sectMobPL", args);
+ sleep(5);
+ initSectionControl();
+ 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
+ info << "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;
+
+}
Modified: trunk/moast-bth/TeamTalkSimulator/imageClient.cc
===================================================================
--- trunk/moast-bth/TeamTalkSimulator/imageClient.cc 2007-12-04 20:52:26 UTC (rev 880)
+++ trunk/moast-bth/TeamTalkSimulator/imageClient.cc 2007-12-04 21:03:07 UTC (rev 881)
@@ -1,219 +1,219 @@
-#include <netdb.h>
-#include <sys/types.h>
-#include <sys/socket.h>
-#include <netinet/in.h>
-#include <sys/socket.h>
-#include <unistd.h>
-#include <fcntl.h> //for connect_with_timeout
-
-#include <iostream>
-
-#include <utils.h>
-
-#include "imageClient.h"
-
-using namespace std;
-
-ImageClientException::ImageClientException() throw() {}
-const char* ImageClientException::what() const throw() {
- return "can't connect to image server";
-}
-
-int ImageClient::connect_with_timeout(int sockfd, const struct sockaddr* serv_addr,
- socklen_t addrlen, int timeout) {
-
- // code borrowed from HAL http://www.developerweb.net/forum/showthread.php?p=13486
-
- // Set non-blocking
- long arg;
- if((arg = fcntl(sockfd, F_GETFL, NULL)) < 0) {
- error << "Error fcntl(..., F_GETFL) (" << strerror(errno) << ')' << endl;
- return errno;
- }
- arg |= O_NONBLOCK;
- if( fcntl(sockfd, F_SETFL, arg) < 0) {
- error << "Error fcntl(..., F_SETFL) (" << strerror(errno) << ')' << endl;
- return errno;
- }
-
- // Trying to connect with timeout
- int res = connect(sockfd, serv_addr, addrlen);
- if (res < 0) {
- if (errno == EINPROGRESS) {
- do {
- fd_set myset;
- struct timeval tv;
- tv.tv_sec = timeout;
- tv.tv_usec = 0;
- FD_ZERO(&myset);
- FD_SET(sockfd, &myset);
- res = select(sockfd+1, NULL, &myset, NULL, &tv);
- if (res < 0 && errno != EINTR) {
- error << "Error connecting " << errno << " - " << strerror(errno) << endl;
- return errno;
- }
- else if (res > 0) {
- // Socket selected for write
- socklen_t lon = sizeof(int);
- int valopt;
- if (getsockopt(sockfd, SOL_SOCKET, SO_ERROR, (void*)(&valopt), &lon) < 0) {
- error << "Error in getsockopt() " << errno << " - " << strerror(errno) << endl;
- return errno;
- }
- // Check the value returned...
- if (valopt) {
- error << "Error in delayed connection() " << errno << " - " << strerror(valopt) << endl;
- return errno;
- }
- break;
- }
- else {
- //error << "Timeout in select() - Cancelling!" << endl;
- return ETIMEDOUT;
- }
- } while (1);
- }
- else {
- error << "Error connecting " << errno << " - " << strerror(errno) << endl;
- return errno;
- }
- }
- // Set to blocking mode again...
- if( (arg = fcntl(sockfd, F_GETFL, NULL)) < 0) {
- error << "Error fcntl(..., F_GETFL) (" << strerror(errno) << ')' << endl;
- return errno;
- }
- arg &= (~O_NONBLOCK);
- if( fcntl(sockfd, F_SETFL, arg) < 0) {
- error << "Error fcntl(..., F_SETFL) (" << strerror(errno) << ')' << endl;
- return errno;
- }
- return 0;
-}
-
-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()))) {
- error << "socket error: cannot find host " << host << endl;
- throw ImageClientException();
- }
-
- 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_with_timeout(sockfd, (struct sockaddr*) &serv_addr, sizeof(serv_addr), 3)) {
- perror("connect");
- throw ImageClientException();
- }
-
- //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()))) {
- error << "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;
- }
-
- //debug << "about to connect" << endl;
- if (connect(sockfd, (struct sockaddr*) &serv_addr, sizeof(serv_addr)) < 0) {
- perror("connect");
- return;
- }
- //debug << "connected" << endl;
-
- //if((*image_size = getImage_()) > 0) image = image_;
-}
-
-ImageClient::~ImageClient() {
- delete image_;
- close(sockfd);
-}
-
-int ImageClient::getImage_() {
- char iType;
- debug << "reading image type" << endl;
- if(!readn(&iType)) {
- error << "problem getting image type" << endl;
- return -1;
- }
- debug << "Image Type is " << (int)iType << endl;
- if((int)iType < 1 || (int)iType > 5) {
- error << "can't deal with image of type " << (int)iType << endl;
- return -1;
- }
- // for names images -- not yet supported
- //char iName[16];
- //if(!readn(&iName)) {
- // error << "problem reading image name" << endl;
- // return -1;
- //}
- //debug << "image name is " << iName << endl;
- if(!readn(&image_size_)) {
- error << "problem getting image size" << endl;
- return -1;
- }
- image_size_ = ntohl(image_size_);
- debug << "image size is " << image_size_ << endl;
- if(image_size_ <= 0) {
- error << "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_)) {
- error << "problem getting image" << endl;
- return -1;
- }
- return image_size_;
-}
-
-int ImageClient::getImage(const unsigned char*& imagePtr) {
- char ack[] = {'O', 'k', '\n', '\r'};
- debug << "sending ack" << endl;
- send(sockfd, ack, 4, 0);
-
- if(getImage_() <= 0) {
- cerr << "cannot get image" << endl;
- return -1;
- }
- imagePtr = image_;
- return image_size_;
-}
-
+#include <netdb.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <sys/socket.h>
+#include <unistd.h>
+#include <fcntl.h> //for connect_with_timeout
+
+#include <iostream>
+
+#include <utils.h>
+
+#include "imageClient.h"
+
+using namespace std;
+
+ImageClientException::ImageClientException() throw() {}
+const char* ImageClientException::what() const throw() {
+ return "can't connect to image server";
+}
+
+int ImageClient::connect_with_timeout(int sockfd, const struct sockaddr* serv_addr,
+ socklen_t addrlen, int timeout) {
+
+ // code borrowed from HAL http://www.developerweb.net/forum/showthread.php?p=13486
+
+ // Set non-blocking
+ long arg;
+ if((arg = fcntl(sockfd, F_GETFL, NULL)) < 0) {
+ error << "Error fcntl(..., F_GETFL) (" << strerror(errno) << ')' << endl;
+ return errno;
+ }
+ arg |= O_NONBLOCK;
+ if( fcntl(sockfd, F_SETFL, arg) < 0) {
+ error << "Error fcntl(..., F_SETFL) (" << strerror(errno) << ')' << endl;
+ return errno;
+ }
+
+ // Trying to connect with timeout
+ int res = connect(sockfd, serv_addr, addrlen);
+ if (res < 0) {
+ if (errno == EINPROGRESS) {
+ do {
+ fd_set myset;
+ struct timeval tv;
+ tv.tv_sec = timeout;
+ tv.tv_usec = 0;
+ FD_ZERO(&myset);
+ FD_SET(sockfd, &myset);
+ res = select(sockfd+1, NULL, &myset, NULL, &tv);
+ if (res < 0 && errno != EINTR) {
+ error << "Error connecting " << errno << " - " << strerror(errno) << endl;
+ return errno;
+ }
+ else if (res > 0) {
+ // Socket selected for write
+ socklen_t lon = sizeof(int);
+ int valopt;
+ if (getsockopt(sockfd, SOL_SOCKET, SO_ERROR, (void*)(&valopt), &lon) < 0) {
+ error << "Error in getsockopt() " << errno << " - " << strerror(errno) << endl;
+ return errno;
+ }
+ // Check the value returned...
+ if (valopt) {
+ error << "Error in delayed connection() " << errno << " - " << strerror(valopt) << endl;
+ return errno;
+ }
+ break;
+ }
+ else {
+ //error << "Timeout in select() - Cancelling!" << endl;
+ return ETIMEDOUT;
+ }
+ } while (1);
+ }
+ else {
+ error << "Error connecting " << errno << " - " << strerror(errno) << endl;
+ return errno;
+ }
+ }
+ // Set to blocking mode again...
+ if( (arg = fcntl(sockfd, F_GETFL, NULL)) < 0) {
+ error << "Error fcntl(..., F_GETFL) (" << strerror(errno) << ')' << endl;
+ return errno;
+ }
+ arg &= (~O_NONBLOCK);
+ if( fcntl(sockfd, F_SETFL, arg) < 0) {
+ error << "Error fcntl(..., F_SETFL) (" << strerror(errno) << ')' << endl;
+ return errno;
+ }
+ return 0;
+}
+
+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()))) {
+ error << "socket error: cannot find host " << host << endl;
+ throw ImageClientException();
+ }
+
+ 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_with_timeout(sockfd, (struct sockaddr*) &serv_addr, sizeof(serv_addr), 3)) {
+ perror("connect");
+ throw ImageClientException();
+ }
+
+ //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()))) {
+ error << "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;
+ }
+
+ //debug << "about to connect" << endl;
+ if (connect(sockfd, (struct sockaddr*) &serv_addr, sizeof(serv_addr)) < 0) {
+ perror("connect");
+ return;
+ }
+ //debug << "connected" << endl;
+
+ //if((*image_size = getImage_()) > 0) image = image_;
+}
+
+ImageClient::~ImageClient() {
+ delete image_;
+ close(sockfd);
+}
+
+int ImageClient::getImage_() {
+ char iType;
+ debug << "reading image type" << endl;
+ if(!readn(&iType)) {
+ error << "problem getting image type" << endl;
+ return -1;
+ }
+ debug << "Image Type is " << (int)iType << endl;
+ if((int)iType < 1 || (int)iType > 5) {
+ error << "can't deal with image of type " << (int)iType << endl;
+ return -1;
+ }
+ // for names images -- not yet supported
+ //char iName[16];
+ //if(!readn(&iName)) {
+ // error << "problem reading image name" << endl;
+ // return -1;
+ //}
+ //debug << "image name is " << iName << endl;
+ if(!readn(&image_size_)) {
+ error << "problem getting image size" << endl;
+ return -1;
+ }
+ image_size_ = ntohl(image_size_);
+ debug << "image size is " << image_size_ << endl;
+ if(image_size_ <= 0) {
+ error << "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_)) {
+ error << "problem getting image" << endl;
+ return -1;
+ }
+ return image_size_;
+}
+
+int ImageClient::getImage(const unsigned char*& imagePtr) {
+ char ack[] = {'O', 'k', '\n', '\r'};
+ debug << "sending ack" << endl;
+ send(sockfd, ack, 4, 0);
+
+ if(getImage_() <= 0) {
+ cerr << "cannot get image" << endl;
+ return -1;
+ }
+ imagePtr = image_;
+ return image_size_;
+}
+
Modified: trunk/moast-bth/TeamTalkSimulator/imageClient.h
===================================================================
--- trunk/moast-bth/TeamTalkSimulator/imageClient.h 2007-12-04 20:52:26 UTC (rev 880)
+++ trunk/moast-bth/TeamTalkSimulator/imageClient.h 2007-12-04 21:03:07 UTC (rev 881)
@@ -1,56 +1,56 @@
-#ifndef IMAGE_CLIENT_H
-#define IMAGE_CLIENT_H
-
-#include <string>
-#include <exception>
-
-#include <utils.h>
-
-#ifndef WIN32
-#include <sys/types.h>
-#include <sys/socket.h>
-#include <netinet/in.h>
-#endif
-
-class ImageClientException : public exception {
- public:
- ImageClientException() throw();
- const char* what() const throw();
-};
-
-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_();
- static int connect_with_timeout(int sockfd, const struct sockaddr* serv_addr,
- socklen_t addrlen, int timeout);
- 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
+#ifndef IMAGE_CLIENT_H
+#define IMAGE_CLIENT_H
+
+#include <string>
+#include <exception>
+
+#include <utils.h>
+
+#ifndef WIN32
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#endif
+
+class ImageClientException : public exception {
+ public:
+ ImageClientException() throw();
+ const char* what() const throw();
+};
+
+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_();
+ static int connect_with_timeout(int sockfd, const struct sockaddr* serv_addr,
+ socklen_t addrlen, int timeout);
+ 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
Modified: trunk/moast-bth/TeamTalkSimulator/qSimulator.pl
===================================================================
--- trunk/moast-bth/TeamTalkSimulator/qSimulator.pl 2007-12-04 20:52:26 UTC (rev 880)
+++ trunk/moast-bth/TeamTalkSimulator/qSimulator.pl 2007-12-04 21:03:07 UTC (rev 881)
@@ -1,75 +1,21 @@
#!/usr/bin/perl
use strict;
-use File::Spec;
-my $SCRIPT='runBotServer.sh';
-my $BOTSCRIPT=File::Spec->rel2abs('runBot.pl');
+require 'util.pl';
+
+my $SCRIPT = 'Simulator.pl';
unlink glob "$SCRIPT.*";
-my @robots = &get_bots('peerfile.txt');
-my $num_bots = scalar @robots;
+my $num_bots = scalar &get_bots('peerfile.txt');
-my $ticket = `qsub -q i686 -l nodes=$num_bots $SCRIPT`;
+&writeINI('simulator.ini', {'DISPLAY' => $ENV{'DISPLAY'}});
+
+my $ticket = `qsub -l nodes=$num_bots $SCRIPT`;
chomp $ticket;
-print "ticket: $ticket$/";
-my $hoststring;
-do {
- sleep 1;
- open(STATUS, "qstat -f $ticket|");
- $hoststring = (split(/ = /,(grep(/exec_host =/, <STATUS>))[0]))[1];
-} until ($hoststring);
-chomp $hoststring;
-my @hosts;
-foreach(split(/\+/, $hoststring)) {
- s|/.*||;
- push @hosts, $_;
-}
-die "Needed $num_bots" if(scalar @hosts != $num_bots);
-for(0..$#robots) {
- ${$robots[$_]}{'host'} = $hosts[$_];
-}
-print &render_bots(@robots);
+print $ticket;
+print STDERR $/;
-foreach(@robots) {
- my %robot = %$_;
- my $cmd = "ssh $robot{'host'} $BOTSCRIPT -type=$robot{'type'} -name=$robot{'name'}";
- print $cmd, $/;
- system $cmd;
-}
+exit;
-sub get_bots {
- my $peerfile = shift;
- open(PEERS, $peerfile) || die "Can't open peerfile: $peerfile";
-
- my @robots;
- while(<PEERS>) {
- next if /^\#/ || !/\S/;
- my @tokens = split;
- my $name = $tokens[0];
- next if $name eq 'optrader' || $name eq 'mapserver';
- my %robot;
- $robot{'name'} = $name;
- my @addr = split(':', $tokens[1]);
- $robot{'ip'} = $addr[0];
- $robot{'port'} = $addr[1] if scalar @addr == 2;
- $robot{'voice'} = $tokens[2];
- $robot{'type'} = $tokens[3];
- push @robots, \%robot;
- }
- return @robots;
-}
-
-sub render_bots {
- my @robots = @_;
- my $ret_string;
- foreach(@robots) {
- my %robot = %{$_};
- for(keys %robot) {
- $ret_string .= "$_: $robot{$_} ";
- }
- $ret_string .= $/;
- }
- return $ret_string;
-}
Modified: trunk/moast-bth/TeamTalkSimulator/robot.cc
===================================================================
--- trunk/moast-bth/TeamTalkSimulator/robot.cc 2007-12-04 20:52:26 UTC (rev 880)
+++ trunk/moast-bth/TeamTalkSimulator/robot.cc 2007-12-04 21:03:07 UTC (rev 881)
@@ -1,695 +1,629 @@
-#include <cmath>
-//#include <process.h>
-
-#include <iostream>
-#include <fstream>
-#include <sstream>
-
-#include <signal.h>
-#include <unistd.h>
-
-#include "robot.h"
-
-#include <utils.h>
-
-#ifndef DEFAULT_NML_FILE
-#define DEFAULT_NML_FILE "moast.nml"
-#endif
-#ifndef DEFAULT_INI_FILE
-#define DEFAULT_INI_FILE "moast.ini"
-#endif
-
-#define THIS_PROCESS "TeamTalkSimulator"
-
-using namespace std;
-
-const unsigned int shift = 6;
-const unsigned int mask = ~0U << (sizeof(unsigned int)*8 - shift);
-extern string moast_bin;
-
-void Robot::abortWorkingMsg()
-{
- if(!working_msg) return;
- server_->sendAborted(working_msg->getTaskID());
- delete working_msg;
- working_msg = NULL;
-}
-
-void Robot::checkWorkingMsg()
-{
- if(!working_msg) return;
- if(work_level == VEHWORK) {
- if(statP->echo_serial_number < work_serial) return;
- else if(statP->echo_serial_number > work_serial) {
- abortWorkingMsg();
- } else if(statP->status == RCS_DONE) {
- debug << "trying to send " << working_msg->getTaskID() << " succeeded" << endl;
- server_->sendDone(working_msg->getTaskID(), true);
- delete working_msg;
- working_msg = NULL;
- } else if(statP->status == RCS_ERROR) {
- server_->sendDone(working_msg->getTaskID(), false);
- delete working_msg;
- working_msg = NULL;
- }
- } else if(work_level == AMWORK) {
- if(amStatP->echo_serial_number < work_serial) return;
- else if(amStatP->echo_serial_number > work_serial) {
- abortWorkingMsg();
- } else if(amStatP->status == RCS_DONE) {
- server_->sendDone(working_msg->getTaskID(), true);
- delete working_msg;
- working_msg = NULL;
- } else if(amStatP->status == RCS_ERROR) {
- server_->sendDone(working_msg->getTaskID(), false);
- delete working_msg;
- working_msg = NULL;
- }
- } else {
- error << "unknown work level" << endl;
- return;
- }
-}
-
-int Robot::cleanupVehMobPLNmlBuffers(void)
-{
- if (NULL != vehMobPLCmdBuf)
- delete vehMobPLCmdBuf;
- if (NULL != vehMobPLStatBuf)
- delete vehMobPLStatBuf;
- if (NULL != vehMobPLSetBuf)
- delete vehMobPLSetBuf;
- if (NULL != vehMobPLCfgBuf)
- delete vehMobPLCfgBuf;
- if (NULL != navDataExtBuf)
- delete navDataExtBuf;
-
- vehMobPLStatBuf = NULL;
- vehMobPLSetBuf = NULL;
- vehMobPLCmdBuf = NULL;
- vehMobPLCfgBuf = NULL;
- navDataExtBuf = NULL;
-
- return 0;
-}
-
-int Robot::initVehMobPLNmlBuffers(char *config_file, int bufnum)
-{
- debug << "cleaning up nmlbuffers" << endl;
- 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 << AM_MOB_JA_CMD_NAME << bufnum;
- amMobJACmdBuf =
- new RCS_CMD_CHANNEL(amMobJA_format, chanName.str().c_str(),
- THIS_PROCESS, config_file);
- }
- if (!amMobJACmdBuf->valid()) {
- delete amMobJACmdBuf;
- amMobJACmdBuf = 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 << AM_MOB_JA_STAT_NAME << bufnum;
- amMobJAStatBuf =
- new RCS_STAT_CHANNEL(amMobJA_format, chanName.str().c_str(),
- THIS_PROCESS, config_file);
- }
- if (!amMobJAStatBuf->valid()) {
- delete amMobJAStatBuf;
- amMobJAStatBuf = 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;
- }
-
- {
- ostringstream chanName;
- chanName << NAV_DATA_EXT_NAME << bufnum;
- navDataExtBuf =
- new NML(navDataExt_format, chanName.str().c_str(),
- THIS_PROCESS, config_file);
- }
- if (!navDataExtBuf->valid()) {
- delete navDataExtBuf;
- navDataExtBuf = 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
- << "am serial number: " << amStatP->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: " << 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();
- me->amMobJAStatBuf->read();
- me->navDataExtBuf->read();
-
- me->printStat(cerr);
-
- debug << "entering for loop" << endl;
- //EnterCriticalSection(&me->CriticalSection);
- for(int i;;i++) {
- //LeaveCriticalSection(&me->CriticalSection);
- do {
- const Boeing::MsgCmd *bmsg;
- while(me->server_->isConnected() &&
- (bmsg = me->server_->getNextMessage())) {
- Msg* msg = Msg::interpretBoeingPacket(string(bmsg->buff, 1000));
- if(!msg) {
- warn << me->sim_index << ": got NULL message" << endl;
- break;
- }
- if(dynamic_cast<MsgReqLocation*>(msg)) {
- //debug << me->sim_index << ": got message: " << msg->render() << endl;
- } else {
- info << me->sim_index << ": got message: " << msg->render() << endl;
- //me->printStat(cerr);
- }
- me->vehMobPLStatBuf->read();
- me->vehMobPLSetBuf->read();
- me->primMobJAStatBuf->read();
- me->amMobJAStatBuf->read();
- me->navDataExtBuf->read();
- me->callback(msg, NULL);
- delete msg;
- }
- sleep(updatePeriod);
- me->vehMobPLStatBuf->read();
- me->vehMobPLSetBuf->read();
- me->primMobJAStatBuf->read();
- me->amMobJAStatBuf->read();
- me->navDataExtBuf->read();
- me->checkWorkingMsg();
- //} 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;
@@ Diff output truncated at 60000 characters. @@
More information about the TeamTalk-developers
mailing list