[TeamTalk 35]: [572] TeamTalk/Agents/TeamTalkSimulator: Added ImageClient exception
tk@edam.speech.cs.cmu.edu
tk at edam.speech.cs.cmu.edu
Thu Apr 5 15:17:43 EDT 2007
An HTML attachment was scrubbed...
URL: http://mailman.srv.cs.cmu.edu/pipermail/teamtalk-developers/attachments/20070405/507b8f35/attachment-0001.html
-------------- next part --------------
Property changes on: TeamTalk/Agents/TeamTalkSimulator
___________________________________________________________________
Name: svn:ignore
+ TeamTalkSimulator *.d
Modified: TeamTalk/Agents/TeamTalkSimulator/Makefile
===================================================================
--- TeamTalk/Agents/TeamTalkSimulator/Makefile 2007-04-05 18:44:23 UTC (rev 571)
+++ TeamTalk/Agents/TeamTalkSimulator/Makefile 2007-04-05 19:17:43 UTC (rev 572)
@@ -30,16 +30,16 @@
EXECUTABLES = TeamTalkSimulator
+all: $(EXECUTABLES)
+
# Include the dependencies
-include $(OBJECTS:.o=.d)
-all: $(EXECUTABLES)
-
-TeamTalkSimulator: primitivecomm $(OBJECTS)
+TeamTalkSimulator: $(PRIMITIVECOMM_DIR)/libboeingrobotserver.a $(OBJECTS)
@rm -f TeamTalkSimulatedBot
$(CXXLINK) $(OBJECTS) $(PRIMITIVECOMM_DIR)/libboeingrobotserver.a $(LDLIBS)
-primitivecomm:
+$(PRIMITIVECOMM_DIR)/libboeingrobotserver.a: force
$(MAKE) -C $(PRIMITIVECOMM_DIR) all
# Compilation rule
@@ -64,3 +64,5 @@
clean:
$(MAKE) -C $(PRIMITIVECOMM_DIR) clean
rm -f *.o $(EXECUTABLES)
+
+force: ;
\ No newline at end of file
Modified: TeamTalk/Agents/TeamTalkSimulator/TeamTalkSimulator.cc
===================================================================
--- TeamTalk/Agents/TeamTalkSimulator/TeamTalkSimulator.cc 2007-04-05 18:44:23 UTC (rev 571)
+++ TeamTalk/Agents/TeamTalkSimulator/TeamTalkSimulator.cc 2007-04-05 19:17:43 UTC (rev 572)
@@ -434,12 +434,11 @@
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;
+ vector<string> args;
+ args.push_back("-r.2");
+ SectMobPLPid = spawn(false, moast_bin, "sectMobPL", args);
+ sleep(5);
initSectionControl();
- //start dumping map
- debug << "<TeamTalkSimulator::main> about to mapDump" << endl;
mapDump();
//start the trader
Modified: TeamTalk/Agents/TeamTalkSimulator/imageClient.cc
===================================================================
--- TeamTalk/Agents/TeamTalkSimulator/imageClient.cc 2007-04-05 18:44:23 UTC (rev 571)
+++ TeamTalk/Agents/TeamTalkSimulator/imageClient.cc 2007-04-05 19:17:43 UTC (rev 572)
@@ -4,6 +4,7 @@
#include <netinet/in.h>
#include <sys/socket.h>
#include <unistd.h>
+#include <fcntl.h> //for connect_with_timeout
#include <iostream>
@@ -13,6 +14,83 @@
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)
{
@@ -21,7 +99,7 @@
if(!(hostptr = gethostbyname(host.c_str()))) {
error << "socket error: cannot find host " << host << endl;
- return;
+ throw ImageClientException();
}
memset((char *) &serv_addr, 0, sizeof(serv_addr));
@@ -36,9 +114,9 @@
return;
}
- if (connect(sockfd, (struct sockaddr*) &serv_addr, sizeof(serv_addr)) < 0) {
+ if (connect_with_timeout(sockfd, (struct sockaddr*) &serv_addr, sizeof(serv_addr), 3)) {
perror("connect");
- return;
+ throw ImageClientException();
}
//getImage_();
@@ -85,27 +163,29 @@
int ImageClient::getImage_() {
char iType;
+ debug << "reading image type" << endl;
if(!readn(&iType)) {
error << "problem getting image type" << endl;
return -1;
}
- //debug << "Imgage Type is " << (int)iType << endl;
+ 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;
}
- char iName[16];
- if(!readn(&iName)) {
- error << "problem reading image name" << endl;
- return -1;
- }
- //cerr << "image name is " << iName << endl;
+ // 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 type" << endl;
+ error << "problem getting image size" << endl;
return -1;
}
image_size_ = ntohl(image_size_);
- //cerr << "image size is " << image_size_ << endl;
+ debug << "image size is " << image_size_ << endl;
if(image_size_ <= 0) {
error << "bad image size: " << image_size_ << endl;
return -1;
@@ -126,7 +206,7 @@
int ImageClient::getImage(const unsigned char*& imagePtr) {
char ack[] = {'O', 'k', '\n', '\r'};
- //debug << "sending" << endl;
+ debug << "sending ack" << endl;
send(sockfd, ack, 4, 0);
if(getImage_() <= 0) {
Modified: TeamTalk/Agents/TeamTalkSimulator/imageClient.h
===================================================================
--- TeamTalk/Agents/TeamTalkSimulator/imageClient.h 2007-04-05 18:44:23 UTC (rev 571)
+++ TeamTalk/Agents/TeamTalkSimulator/imageClient.h 2007-04-05 19:17:43 UTC (rev 572)
@@ -2,6 +2,7 @@
#define IMAGE_CLIENT_H
#include <string>
+#include <exception>
#include <utils.h>
@@ -11,6 +12,12 @@
#include <netinet/in.h>
#endif
+class ImageClientException : public exception {
+ public:
+ ImageClientException() throw();
+ const char* what() const throw();
+};
+
class ImageClient {
protected:
int sockfd;
@@ -36,6 +43,8 @@
};
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);
Modified: TeamTalk/Agents/TeamTalkSimulator/robot.cc
===================================================================
--- TeamTalk/Agents/TeamTalkSimulator/robot.cc 2007-04-05 18:44:23 UTC (rev 571)
+++ TeamTalk/Agents/TeamTalkSimulator/robot.cc 2007-04-05 19:17:43 UTC (rev 572)
@@ -27,6 +27,50 @@
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)
@@ -82,6 +126,19 @@
{
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(),
@@ -108,6 +165,19 @@
{
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(),
@@ -176,6 +246,7 @@
<< 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
@@ -235,63 +306,14 @@
Robot* me = (Robot*) thisp;
sleep(5);
-
me->vehMobPLStatBuf->read();
me->vehMobPLSetBuf->read();
me->primMobJAStatBuf->read();
+ me->amMobJAStatBuf->read();
+ me->navDataExtBuf->read();
- debug << "command initialization" << endl;
- 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);
-
debug << "entering for loop" << endl;
//EnterCriticalSection(&me->CriticalSection);
for(int i;;i++) {
@@ -302,17 +324,30 @@
(bmsg = me->server_->getNextMessage())) {
Msg* msg = Msg::interpretBoeingPacket(string(bmsg->buff, 1000));
if(!msg) {
- warn << "got NULL message" << endl;
+ warn << me->sim_index << ": got NULL message" << endl;
break;
}
if(dynamic_cast<MsgReqLocation*>(msg)) {
- //debug << "got message: " << msg->render() << endl;
+ //debug << me->sim_index << ": got message: " << msg->render() << endl;
} else {
- info << "got message: " << msg->render() << endl;
+ 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);
/*
@@ -391,7 +426,9 @@
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)
+ vehMobPLCfgBuf(NULL), vehMobPLSetBuf(NULL), primMobJACmdBuf(NULL),
+ amMobJACmdBuf(NULL), primMobJAStatBuf(NULL), amMobJAStatBuf(NULL),
+ working_msg(NULL)
{
//InitializeCriticalSection(&CriticalSection);
sim_init();
@@ -433,12 +470,13 @@
debug << "set up convenient pointers" << endl;
statP = (VehMobPLStat *) vehMobPLStatBuf->get_address();
primStatP = (PrimMobJAStat *) primMobJAStatBuf->get_address();
+ amStatP = (AmMobJAStat *) amMobJAStatBuf->get_address();
setP = (VehMobPLSet *) vehMobPLSetBuf->get_address();
navP = (NavDataExt *) navDataExtBuf->get_address();
debug << "new robotserver" << endl;
server_ = new Boeing::RobotServer();
- debug << "opening port" << endl;
+ debug << "opening port: " << port << endl;
if (port) server_->open(port);
else server_->open();
debug << "starting reading thread" << endl;
@@ -457,67 +495,58 @@
}
void Robot::callback(const Msg* msgp, const sockaddr* st_remote) {
- //print_debugln("got a message");
- vehMobPLStatBuf->read();
- vehMobPLSetBuf->read();
- primMobJAStatBuf->read();
- navDataExtBuf->read();
updatePose();
- //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 MsgMapReq* req_image = dynamic_cast<const MsgMapReq*>(msgp)) {
- //print_debugln("got an req image");
+ debug << "got an req image" << endl;
//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);
+ try {
+ if(imageClient == NULL) {
+ imageClient = new ImageClient("virbot.speech.cs.cmu.edu",
+ (unsigned short)5003);
+ }
+ debug << "getting the image" << endl;
+ image_size = imageClient->getImage(image);
+ debug << "got the image" << endl;
+
+ if(image_size > 0) {
+ //send the image to the robot client
+ server_->sendJPEGImage(image, image_size, 320, 240, req_image->getInvoice());
+ } else {
+ error << "Image size is 0" << endl;
+ }
+ } catch (ImageClientException e) {
+ error << "ImageClientException: " << e.what() << endl;
}
- //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");
+ debug << "done with req image" << endl;
return;
}
if(dynamic_cast<const MsgReqLocation*>(msgp)) {
- //print_debugln("got an req location");
- server_->sendLocation(getX(), -getY(), canonical_angle(-getYaw()), true);
- //delete temp_message;
+ server_->sendLocation(getX(), getY(), canonical_angle(getYaw()), true);
return;
}
//EnterCriticalSection(&CriticalSection);
if(const MsgCmdGoTo* goTo = dynamic_cast<const MsgCmdGoTo*>(msgp)) {
debug << "got: " << goTo << endl;
- Point<float> vec(goTo->getX(), goTo->getY());
+ 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()));
- rotate.thetaTol = PI/20; //9 degrees tolerance
- // always make it a new command
- rotate.serial_number = primStatP->echo_serial_number + 1;
- primMobJACmdBuf->write(&rotate);
+ debug << "interpreting as turn relative: " << goTo->getAngle() << endl;
+ float r = getYaw();
+ AmMobJACmdSpin spin;
+ spin.absAngle = canonical_angle(r - goTo->getAngle());
+ spin.tolerance = PI/40;
+ spin.direction = PRIM_MOB_JA_CMD_ROTATE_SHORTEST;
+ spin.serial_number = amStatP->echo_serial_number + 1;
+ debug << "spinning: " << spin.absAngle << " tol: " << spin.tolerance
+ << " SHORTEST" << endl;
+ amMobJACmdBuf->write(&spin);
+ setNewWorkingMsg(goTo, AMWORK, spin.serial_number);
} else {
Point<float> loc(getX(), getY());
Point<float> destination;
@@ -537,13 +566,12 @@
move.neighborhood = 1;
move.startTime = 0;
vehMobPLCmdBuf->write(&move);
+ setNewWorkingMsg(goTo, VEHWORK, move.serial_number);
}
- //delete temp_message;
return;
} else if(const MsgCmdHalt* halt = dynamic_cast<const MsgCmdHalt*>(msgp)) {
debug << "got: " << halt << endl;
action = WAITING;
- //delete temp_message;
return;
} else if(const MsgCmdHome* home = dynamic_cast<const MsgCmdHome*>(msgp)) {
debug << "got: " << home << endl;
@@ -555,7 +583,6 @@
move.neighborhood = 1;
move.startTime = 0;
vehMobPLCmdBuf->write(&move);
- //delete temp_message;
return;
} else if(const MsgCmdPause* pause = dynamic_cast<const MsgCmdPause*>(msgp)) {
debug << "got: " << pause << endl;
@@ -573,7 +600,6 @@
//setFollow(follow->content()->leader);
}
//LeaveCriticalSection(&CriticalSection);
- //delete temp_message;
}
/**
@@ -594,7 +620,7 @@
args.push_back("-p2");
//args.push_back("-v");
spawn(false, moast_bin, "simWare", args);
- sleep(20);
+ sleep(5);
args.clear();
{
@@ -603,7 +629,7 @@
args.push_back(arg.str());
}
pids.push_back(spawn(false, moast_bin, "slamStub", args));
- sleep(2);
+ sleep(1);
args.clear();
{
@@ -615,7 +641,6 @@
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();
@@ -629,7 +654,7 @@
args.clear();
args.push_back("-i");
- args.push_back("-g3");
+ args.push_back("-g1");
args.push_back("-r.2");
{
ostringstream arg;
Modified: TeamTalk/Agents/TeamTalkSimulator/robot.h
===================================================================
--- TeamTalk/Agents/TeamTalkSimulator/robot.h 2007-04-05 18:44:23 UTC (rev 571)
+++ TeamTalk/Agents/TeamTalkSimulator/robot.h 2007-04-05 19:17:43 UTC (rev 572)
@@ -12,6 +12,7 @@
#include <moastNmlOffsets.hh> // NAV_DATA_BASE, etc.
#include <vehMobPL.hh> // vehMobPL_format()
#include <primMobJA.hh> // primMobJA_format()
+#include <amMobJA.hh> // amMobJA_format()
#include <navDataExt.hh> // navigation data
#include "imageClient.h"
@@ -39,17 +40,36 @@
//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;
+ RCS_CMD_CHANNEL *primMobJACmdBuf;
+ RCS_CMD_CHANNEL *amMobJACmdBuf;
+ RCS_STAT_CHANNEL *primMobJAStatBuf;
+ RCS_STAT_CHANNEL *amMobJAStatBuf;
NML *navDataExtBuf;
VehMobPLStat *statP;
VehMobPLSet *setP;
PrimMobJAStat *primStatP;
+ AmMobJAStat *amStatP;
NavDataExt *navP;
Pose<float> pose;
+ //command status
+ MsgCmd* working_msg;
+ typedef enum {VEHWORK, AMWORK} work_level_t;
+ work_level_t work_level;
+ int work_serial;
+ template <class C>
+ void setNewWorkingMsg(const C* wm, work_level_t wl, int sn)
+ {
+ if(working_msg) abortWorkingMsg();
+ working_msg = new C(*wm);
+ work_level = wl;
+ work_serial = sn;
+ }
+ void abortWorkingMsg();
+ void checkWorkingMsg();
+
int cleanupVehMobPLNmlBuffers();
int initVehMobPLNmlBuffers(char *config, int bufnum);
static const int updatePeriod = 1;
Modified: TeamTalk/Agents/TeamTalkSimulator/runSimulator.sh
===================================================================
--- TeamTalk/Agents/TeamTalkSimulator/runSimulator.sh 2007-04-05 18:44:23 UTC (rev 571)
+++ TeamTalk/Agents/TeamTalkSimulator/runSimulator.sh 2007-04-05 19:17:43 UTC (rev 572)
@@ -13,119 +13,5 @@
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
+./TeamTalkSimulator --moast $MOAST --peerfile peerfile.txt
-# 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
More information about the TeamTalk-developers
mailing list