[TeamTalk 28]: [565] TeamTalk/Agents/TeamTalkSimulator: linux builds automatically handle depenencies
tk@edam.speech.cs.cmu.edu
tk at edam.speech.cs.cmu.edu
Thu Dec 21 01:16:02 EST 2006
An HTML attachment was scrubbed...
URL: http://mailman.srv.cs.cmu.edu/pipermail/teamtalk-developers/attachments/20061221/ddb21569/attachment-0001.html
-------------- next part --------------
Modified: TeamTalk/Agents/TeamTalkSimulator/Makefile
===================================================================
--- TeamTalk/Agents/TeamTalkSimulator/Makefile 2006-12-21 06:00:03 UTC (rev 564)
+++ TeamTalk/Agents/TeamTalkSimulator/Makefile 2006-12-21 06:16:02 UTC (rev 565)
@@ -1,5 +1,5 @@
-MOASTLIB_DIR = /net/eucalyptus/usr0/tkharris/moast-1.2
-RCSLIB_DIR = /net/eucalyptus/usr0/tkharris/lib/rcslib
+MOASTLIB_DIR = $(CARTOON_HOME)/moast/devel
+RCSLIB_DIR = $(CARTOON_HOME)/lib/rcslib
BOEINGLIB_DIR = ../boeingLib
PRIMITIVECOMM_DIR = ../PrimitiveComm
@@ -10,34 +10,57 @@
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 \
- $(PRIMITIVECOMM_DIR)/libboeingrobotserver.a
+OBJECTS = TeamTalkSimulator.o robot.o imageClient.o
EXECUTABLES = TeamTalkSimulator
+# Include the dependencies
+-include $(OBJECTS:.o=.d)
+
all: $(EXECUTABLES)
TeamTalkSimulator: primitivecomm $(OBJECTS)
@rm -f TeamTalkSimulatedBot
- $(CXXLINK) $(OBJECTS) $(LDLIBS)
+ $(CXXLINK) $(OBJECTS) $(PRIMITIVECOMM_DIR)/libboeingrobotserver.a $(LDLIBS)
primitivecomm:
- $(MAKE) -C $(PRIMITIVECOMM_DIR)
+ $(MAKE) -C $(PRIMITIVECOMM_DIR) all
-TeamTalkSimulator.o: TeamTalkSimulator.cc robot.h imageClient.h $(PRIMITIVECOMM_DIR)/utils.h
- $(CXXCOMPILE) -o $@ $<
+# Compilation rule
+%.o: %.cc
+ $(CC_ECHO)
+ $(Q)$(CXXCOMPILE) -o $@ $<
-robot.o: robot.cc robot.h imageClient.h $(PRIMITIVECOMM_DIR)/utils.h
- $(CXXCOMPILE) -o $@ $<
+%.o: %.cpp
+ $(CC_ECHO)
+ $(Q)$(CXXCOMPILE) -o $@ $<
-imageClient.o: imageClient.cc imageClient.h $(PRIMITIVECOMM_DIR)/utils.h
- $(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 $(EXECUTABLES)
Modified: TeamTalk/Agents/TeamTalkSimulator/TeamTalkSimulator.cc
===================================================================
--- TeamTalk/Agents/TeamTalkSimulator/TeamTalkSimulator.cc 2006-12-21 06:00:03 UTC (rev 564)
+++ TeamTalk/Agents/TeamTalkSimulator/TeamTalkSimulator.cc 2006-12-21 06:16:02 UTC (rev 565)
@@ -39,16 +39,15 @@
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"
+#define DEFAULT_NML_FILE "moast.nml"
#endif
#ifndef DEFAULT_INI_FILE
-#define DEFAULT_INI_FILE "/net/eucalyptus/usr0/tkharris/moast-1.2/etc/moast.ini"
+#define DEFAULT_INI_FILE "moast.ini"
#endif
#define THIS_PROCESS "sectShell"
+static const char* MAP_DUMP = "/tmp/SectMobPLDispOutput.ppm";
char nml_file_default[] = DEFAULT_NML_FILE;
char *nml_file_env;
@@ -58,6 +57,8 @@
char *INI_FILE = NULL;
int bufnum = 1;
+string moast_bin;
+
/****************** MAP STUFF *********************/
static unsigned char* conveyedMap;
@@ -77,9 +78,9 @@
Boeing::MsgMap::MsgMapFactory(Boeing::MAP_RLE,
msgMap, currentMap, mapWidth*mapHeight,
0, 0, mapWidth, mapHeight);
- cerr << "raw: " << msgMap->map[0]
- << " rl: " << ((msgMap->map[0]&0xFFFFFF00)>>8)
- << " rv: " << (msgMap->map[0]&0x000000FF) << endl;
+ //debug << "raw: " << msgMap->map[0]
+ // << " rl: " << ((msgMap->map[0]&0xFFFFFF00)>>8)
+ // << " rv: " << (msgMap->map[0]&0x000000FF) << endl;
mserver->sendFullMap(msgMap);
}
@@ -88,9 +89,9 @@
Boeing::MsgMap::MsgMapFactory(Boeing::MAP_RLE,
msgMap, diffMap, mapWidth*mapHeight,
0, 0, mapWidth, mapHeight);
- cerr << "raw: " << msgMap->map[0]
- << " rl: " << ((msgMap->map[0]&0xFFFFFF00)>>8)
- << " rv: " << (msgMap->map[0]&0x000000FF) << endl;
+ //debug << "raw: " << msgMap->map[0]
+ // << " rl: " << ((msgMap->map[0]&0xFFFFFF00)>>8)
+ // << " rv: " << (msgMap->map[0]&0x000000FF) << endl;
mserver->sendDiffMap(msgMap);
}
@@ -115,12 +116,14 @@
subscribed = false;
break;
default:
- cerr << "unknown or unhandled msgtype: " << msg->hdr.type << endl;
+ error << "unknown or unhandled msgtype: " << msg->hdr.type << endl;
}
} else {
- if(subscribed) sendDiffMap(mserver);
- sleep(updatePeriod);
+ if(subscribed)
+ //sendDiffMap(mserver);
+ sendFullMap(mserver);
}
+ sleep(updatePeriod);
}
}
@@ -131,7 +134,7 @@
for(;;) {
const Boeing::MsgTraderServer *msg = tserver->getNextMessage();
if(!msg) {
- //cerr << "getNextMessage is NULL" << endl;
+ //debug << "getNextMessage is NULL" << endl;
sleep(1000);
continue;
}
@@ -140,15 +143,15 @@
sectMobPLSetBuf->read();
if(msg->hdr.type != Boeing::TRADER_TASK) {
- cerr << "don't know what to do with header type " << msg->hdr.type << endl;
+ error << "don't know what to do with header type " << msg->hdr.type << endl;
continue;
}
- cerr << "got: " << msg->msg_task.task << endl;
+ debug << "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;
+ error << "don't know how to deal with command " << token << endl;
continue;
}
float x, y;
@@ -163,13 +166,51 @@
}
void readMapData() {
- ifstream f("stubdb.txt");
- f >> mapWidth >> mapHeight;
+ ifstream f(MAP_DUMP);
+ 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++) {
- int n;
- f >> n;
- currentMap[j*mapWidth+i] = (unsigned char)n;
+ 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;
+ }
}
}
}
@@ -214,7 +255,7 @@
dump.serial_number = sectMobPLSetPtr->echo_serial_number + 1;
dump.skip = 5;
dump.dumpContinuous = true;
- strcpy(dump.fileName, "/tmp/SectMobPLDispOutput.ppm");
+ strcpy(dump.fileName, MAP_DUMP);
sectMobPLCfgBuf->write(&dump);
}
@@ -300,7 +341,7 @@
int retval = initSectMobPLNmlBuffers(NML_FILE, bufnum);
if (retval) {
- cerr << "TeamTalkSimulator: Can't allocate NML buffers" << endl;
+ fatal << "TeamTalkSimulator: Can't allocate NML buffers" << endl;
return 1;
}
@@ -315,6 +356,7 @@
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;
}
@@ -325,15 +367,11 @@
int main(int argc, char *argv[])
{
+ DebugStream::threashold_ = DebugStream::D; //give me all debug messages
+
//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++) {
@@ -343,18 +381,29 @@
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 {
- //start a robot, put it on the stack
- //robots[argv[i]] = new Robot(&robots, argv[i], atoi(argv[i]), 1);
+ 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");
+ NmlSvrPid = spawn(false, moast_bin, "moastNmlSvr");
+ sleep(3);
+
if(!peerfile.empty()) {
int robot_count = 1;
- cerr << "adding robots from " << peerfile << endl;
+ debug << "adding robots from " << peerfile << endl;
ifstream Frobotips(peerfile.c_str());
if(!Frobotips) {
- cerr << "problem opening " << peerfile << endl;
+ error << "problem opening " << peerfile << endl;
throw exception();
}
string rname;
@@ -374,10 +423,10 @@
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;
+ debug << "added " << rname << '@' << port << endl;
} else {
robots[rname] = new Robot(&robots, rname, robot_count++);
- cerr << "added " << rname << endl;
+ debug << "added " << rname << endl;
}
ignoreToEndOfLine(Frobotips);
}
@@ -385,8 +434,12 @@
if(robots.empty()) robots[name] = new Robot(&robots, name, 1);
//initialize section control
+ SectMobPLPid = spawn(false, moast_bin, "sectMobPL");
+ sleep(20);
+ debug << "<TeamTalkSimulator::main> about to init Section Control" << endl;
initSectionControl();
//start dumping map
+ debug << "<TeamTalkSimulator::main> about to mapDump" << endl;
mapDump();
//start the trader
@@ -405,7 +458,7 @@
pthread_create(&mapThread_, NULL, readMapRequests, (void *) &mserver);
//wait for all of the threads to terminate
- cerr << "wait for thread termination...\n" << endl;
+ 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
Modified: TeamTalk/Agents/TeamTalkSimulator/imageClient.cc
===================================================================
--- TeamTalk/Agents/TeamTalkSimulator/imageClient.cc 2006-12-21 06:00:03 UTC (rev 564)
+++ TeamTalk/Agents/TeamTalkSimulator/imageClient.cc 2006-12-21 06:16:02 UTC (rev 565)
@@ -20,7 +20,7 @@
struct hostent *hostptr;
if(!(hostptr = gethostbyname(host.c_str()))) {
- cerr << "socket error: cannot find host " << host << endl;
+ error << "socket error: cannot find host " << host << endl;
return;
}
@@ -52,7 +52,7 @@
struct hostent *hostptr;
if(!(hostptr = gethostbyname(host.c_str()))) {
- cerr << "socket error: cannot find host " << host << endl;
+ error << "socket error: cannot find host " << host << endl;
return;
}
@@ -68,12 +68,12 @@
return;
}
- //cerr << "about to connect" << endl;
+ //debug << "about to connect" << endl;
if (connect(sockfd, (struct sockaddr*) &serv_addr, sizeof(serv_addr)) < 0) {
perror("connect");
return;
}
- //cerr << "connected" << endl;
+ //debug << "connected" << endl;
//if((*image_size = getImage_()) > 0) image = image_;
}
@@ -86,28 +86,28 @@
int ImageClient::getImage_() {
char iType;
if(!readn(&iType)) {
- cerr << "problem getting image type" << endl;
+ error << "problem getting image type" << endl;
return -1;
}
- //cerr << "Imgage Type is " << (int)iType << endl;
+ //debug << "Imgage Type is " << (int)iType << endl;
if((int)iType < 1 || (int)iType > 5) {
- cerr << "can't deal with image of type " << (int)iType << endl;
+ error << "can't deal with image of type " << (int)iType << endl;
return -1;
}
char iName[16];
if(!readn(&iName)) {
- cerr << "problem reading image name" << endl;
+ error << "problem reading image name" << endl;
return -1;
}
//cerr << "image name is " << iName << endl;
if(!readn(&image_size_)) {
- cerr << "problem getting image type" << endl;
+ error << "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;
+ error << "bad image size: " << image_size_ << endl;
return -1;
}
@@ -118,7 +118,7 @@
image_ = (unsigned char*)realloc(image_, max_image_size_ = image_size_);
}
if(!readn(image_, image_size_)) {
- cerr << "problem getting image" << endl;
+ error << "problem getting image" << endl;
return -1;
}
return image_size_;
@@ -126,7 +126,7 @@
int ImageClient::getImage(const unsigned char*& imagePtr) {
char ack[] = {'O', 'k', '\n', '\r'};
- //cerr << "sending" << endl;
+ //debug << "sending" << endl;
send(sockfd, ack, 4, 0);
if(getImage_() <= 0) {
Modified: TeamTalk/Agents/TeamTalkSimulator/robot.cc
===================================================================
--- TeamTalk/Agents/TeamTalkSimulator/robot.cc 2006-12-21 06:00:03 UTC (rev 564)
+++ TeamTalk/Agents/TeamTalkSimulator/robot.cc 2006-12-21 06:16:02 UTC (rev 565)
@@ -8,26 +8,24 @@
#include <signal.h>
#include <unistd.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"
+#define DEFAULT_NML_FILE "moast.nml"
#endif
#ifndef DEFAULT_INI_FILE
-#define DEFAULT_INI_FILE "/net/eucalyptus/usr0/tkharris/moast-1.2/etc/moast.ini"
+#define DEFAULT_INI_FILE "moast.ini"
#endif
-#define THIS_PROCESS "vehShell"
+#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;
int Robot::cleanupVehMobPLNmlBuffers(void)
{
@@ -39,11 +37,14 @@
delete vehMobPLSetBuf;
if (NULL != vehMobPLCfgBuf)
delete vehMobPLCfgBuf;
+ if (NULL != navDataExtBuf)
+ delete navDataExtBuf;
vehMobPLStatBuf = NULL;
vehMobPLSetBuf = NULL;
vehMobPLCmdBuf = NULL;
vehMobPLCfgBuf = NULL;
+ navDataExtBuf = NULL;
return 0;
}
@@ -130,7 +131,20 @@
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;
}
@@ -173,7 +187,7 @@
<< "actual cycleTime: " << statP->cycleTime << endl
<< "plan cost: " << statP->planCost << endl
<< "debug: " << setP->debug << endl
- << "pose: " << statP->pose << endl
+ << "pose: " << pose << endl
<< "message: " << statP->message << endl;
}
@@ -291,10 +305,11 @@
warn << "got NULL message" << endl;
break;
}
- debug << "got message: " << msg->render() << endl;
- //if(!dynamic_cast<MsgReqLocationPP*>(msg)) {
- // print_debug("got message: "); print_debugln(msg->render());
- //}
+ if(dynamic_cast<MsgReqLocation*>(msg)) {
+ //debug << "got message: " << msg->render() << endl;
+ } else {
+ info << "got message: " << msg->render() << endl;
+ }
me->callback(msg, NULL);
}
sleep(updatePeriod);
@@ -360,9 +375,17 @@
}
}
-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;}
+float Robot::getX() const {return pose.loc.x;}
+float Robot::getY() const {return pose.loc.y;}
+//float Robot::getYaw() const {return PM_RPY(statP->pose.rot).y;}
+float Robot::getYaw() const {return pose.yaw;}
+Pose<float> Robot::getPose() const {return pose;}
+void Robot::updatePose()
+{
+ pose.loc.x = navP->tranAbs.x;
+ pose.loc.y = navP->tranAbs.y;
+ pose.yaw = navP->rpyAbs.y;
+}
Robot::Robot(const map<string, Robot*> *robs,
string name, int simulator_index, unsigned int port)
@@ -411,6 +434,7 @@
statP = (VehMobPLStat *) vehMobPLStatBuf->get_address();
primStatP = (PrimMobJAStat *) primMobJAStatBuf->get_address();
setP = (VehMobPLSet *) vehMobPLSetBuf->get_address();
+ navP = (NavDataExt *) navDataExtBuf->get_address();
debug << "new robotserver" << endl;
server_ = new Boeing::RobotServer();
@@ -437,8 +461,10 @@
vehMobPLStatBuf->read();
vehMobPLSetBuf->read();
primMobJAStatBuf->read();
+ navDataExtBuf->read();
+ updatePose();
- Msg* temp_message;
+ //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
@@ -446,7 +472,7 @@
//} else {
// temp_message = msgp->clone();
//}
- if(const MsgMapReq* req_image = dynamic_cast<const MsgMapReq*>(temp_message)) {
+ if(const MsgMapReq* req_image = dynamic_cast<const MsgMapReq*>(msgp)) {
//print_debugln("got an req image");
//get the jpeg from the usarsim video socket
const unsigned char* image;
@@ -465,21 +491,26 @@
} else {
cerr << "Image size is 0" << endl;
}
- delete temp_message;
+ //delete temp_message;
//print_debugln("done with req image");
return;
}
- if(dynamic_cast<const MsgReqLocation*>(temp_message)) {
+ if(dynamic_cast<const MsgReqLocation*>(msgp)) {
//print_debugln("got an req location");
server_->sendLocation(getX(), -getY(), canonical_angle(-getYaw()), true);
- delete temp_message;
+ //delete temp_message;
return;
}
//EnterCriticalSection(&CriticalSection);
- if(const MsgCmdGoTo* goTo = dynamic_cast<const MsgCmdGoTo*>(temp_message)) {
- cout << "got: " << goTo << endl;
+ if(const MsgCmdGoTo* goTo = dynamic_cast<const MsgCmdGoTo*>(msgp)) {
+ debug << "got: " << goTo << endl;
Point<float> vec(goTo->getX(), goTo->getY());
+ if(goTo->useAngle()) debug << "useAngle" << endl;
+ else debug << "!useAngle" << endl;
+ if(!vec) debug << "!vec" << endl;
+ else debug << "vec" << endl;
if(goTo->useAngle() && !vec) {
+ debug << "interpreting as turn relative" << endl;
PrimMobJACmdRotate rotate = PrimMobJACmdRotate();
float r = -getYaw();
rotate.theta = canonical_angle(-(r+goTo->getAngle()));
@@ -491,8 +522,10 @@
Point<float> loc(getX(), getY());
Point<float> destination;
if(goTo->isAbsolute()) {
+ debug << "interpreting as goto absolute" << endl;
destination = vec;
} else {
+ debug << "interpreting as goto relative" << endl;
destination =
loc + Point<float>::Polar(vec.length(), getYaw()+vec.angle());
}
@@ -505,15 +538,15 @@
move.startTime = 0;
vehMobPLCmdBuf->write(&move);
}
- delete temp_message;
+ //delete temp_message;
return;
- } else if(const MsgCmdHalt* halt = dynamic_cast<const MsgCmdHalt*>(temp_message)) {
- cout << "got: " << halt << endl;
+ } else if(const MsgCmdHalt* halt = dynamic_cast<const MsgCmdHalt*>(msgp)) {
+ debug << "got: " << halt << endl;
action = WAITING;
- delete temp_message;
+ //delete temp_message;
return;
- } else if(const MsgCmdHome* home = dynamic_cast<const MsgCmdHome*>(temp_message)) {
- cout << "got: " << home << endl;
+ } else if(const MsgCmdHome* home = dynamic_cast<const MsgCmdHome*>(msgp)) {
+ debug << "got: " << home << endl;
VehMobPLCmdMoveWaypoint move = VehMobPLCmdMoveWaypoint();
move.p.x = 0;
move.p.y = 0;
@@ -522,25 +555,25 @@
move.neighborhood = 1;
move.startTime = 0;
vehMobPLCmdBuf->write(&move);
- delete temp_message;
+ //delete temp_message;
return;
- } else if(const MsgCmdPause* pause = dynamic_cast<const MsgCmdPause*>(temp_message)) {
- cout << "got: " << pause << endl;
+ } else if(const MsgCmdPause* pause = dynamic_cast<const MsgCmdPause*>(msgp)) {
+ debug << "got: " << pause << endl;
if(action != PAUSED) {
paused_action = action;
action = PAUSED;
}
- } else if(const MsgCmdResume* resume = dynamic_cast<const MsgCmdResume*>(temp_message)) {
- cout << "got: " << resume << endl;
+ } else if(const MsgCmdResume* resume = dynamic_cast<const MsgCmdResume*>(msgp)) {
+ debug << "got: " << resume << endl;
if(action == PAUSED) {
action = paused_action;
}
- } else if(const MsgCmdFollow* follow = dynamic_cast<const MsgCmdFollow*>(temp_message)) {
- cout << "got: " << follow << endl;
+ } else if(const MsgCmdFollow* follow = dynamic_cast<const MsgCmdFollow*>(msgp)) {
+ debug << "got: " << follow << endl;
//setFollow(follow->content()->leader);
}
//LeaveCriticalSection(&CriticalSection);
- delete temp_message;
+ //delete temp_message;
}
/**
@@ -553,27 +586,37 @@
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("-p2");
//args.push_back("-v");
- spawn(false, MOAST_BIN, "usarSim", args);
- sleep(60);
+ spawn(false, moast_bin, "simWare", args);
+ sleep(20);
args.clear();
{
ostringstream arg;
+ arg << "-i" << sim_index;
+ args.push_back(arg.str());
+ }
+ pids.push_back(spawn(false, moast_bin, "slamStub", args));
+ sleep(2);
+
+ 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));
+ //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));
+ pids.push_back(spawn(false, moast_bin, "amMobMain", args));
args.clear();
args.push_back("-z.1");
@@ -582,7 +625,7 @@
arg << "-d" << sim_index;
args.push_back(arg.str());
}
- pids.push_back(spawn(false, MOAST_BIN, "amSPMain", args));
+ pids.push_back(spawn(false, moast_bin, "amSPMain", args));
args.clear();
args.push_back("-i");
@@ -593,7 +636,7 @@
arg << "-d" << sim_index;
args.push_back(arg.str());
}
- pids.push_back(spawn(false, MOAST_BIN, "vehMobPLMain", args));
+ pids.push_back(spawn(false, moast_bin, "vehMobPLMain", args));
sleep(6);
}
Modified: TeamTalk/Agents/TeamTalkSimulator/robot.h
===================================================================
--- TeamTalk/Agents/TeamTalkSimulator/robot.h 2006-12-21 06:00:03 UTC (rev 564)
+++ TeamTalk/Agents/TeamTalkSimulator/robot.h 2006-12-21 06:16:02 UTC (rev 565)
@@ -12,10 +12,12 @@
#include <moastNmlOffsets.hh> // NAV_DATA_BASE, etc.
#include <vehMobPL.hh> // vehMobPL_format()
#include <primMobJA.hh> // primMobJA_format()
+#include <navDataExt.hh> // navigation data
#include "imageClient.h"
using namespace std;
+using namespace geometry;
//class Robot : public CallBackClass {
class Robot {
@@ -41,9 +43,12 @@
RCS_STAT_CHANNEL *primMobJAStatBuf;
RCS_CMD_CHANNEL *vehMobPLCfgBuf;
RCS_STAT_CHANNEL *vehMobPLSetBuf;
+ NML *navDataExtBuf;
VehMobPLStat *statP;
VehMobPLSet *setP;
PrimMobJAStat *primStatP;
+ NavDataExt *navP;
+ Pose<float> pose;
int cleanupVehMobPLNmlBuffers();
int initVehMobPLNmlBuffers(char *config, int bufnum);
@@ -55,6 +60,8 @@
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
+ Pose<float> getPose() const;
+ void updatePose();
public:
Robot(const map<string, Robot*>* robots,
Modified: TeamTalk/Agents/TeamTalkSimulator/runSimulator.sh
===================================================================
--- TeamTalk/Agents/TeamTalkSimulator/runSimulator.sh 2006-12-21 06:00:03 UTC (rev 564)
+++ TeamTalk/Agents/TeamTalkSimulator/runSimulator.sh 2006-12-21 06:16:02 UTC (rev 565)
@@ -10,9 +10,10 @@
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
+MOAST=${CARTOON_HOME}/moast/devel
+export CONFIG_NML=${MOAST}/etc/moast.nml
+export CONFIG_INI=${MOAST}/etc/moast.ini
+./TeamTalkSimulator --moast $MOAST --peerfile peerfile.txt --mapserver
# cd ../../../moast-1.2/bin
# HOME=/net/eucalyptus/usr0/tkharris
More information about the TeamTalk-developers
mailing list