[TeamTalk 7]: [544] TeamTalk/Agents: Added setpos interpretation for Action message class .
tk@edam.speech.cs.cmu.edu
tk at edam.speech.cs.cmu.edu
Thu Nov 9 14:17:31 EST 2006
An HTML attachment was scrubbed...
URL: http://mailman.srv.cs.cmu.edu/pipermail/teamtalk-developers/attachments/20061109/6ae1667d/attachment-0001.html
-------------- next part --------------
Modified: TeamTalk/Agents/PrimitiveComm/robot_packet.cpp
===================================================================
--- TeamTalk/Agents/PrimitiveComm/robot_packet.cpp 2006-11-08 20:05:26 UTC (rev 543)
+++ TeamTalk/Agents/PrimitiveComm/robot_packet.cpp 2006-11-09 19:17:31 UTC (rev 544)
@@ -59,6 +59,7 @@
}
} else if (w1 == "pause") retval = new MsgCmdPausePP();
else if (w1 == "resume") retval = new MsgCmdResumePP();
+ else if (w1 == "setpos") retval = new MsgCmdSetPos(playaction);
return retval;
}
@@ -88,8 +89,6 @@
case CMD_PAUSE: retval = new MsgCmdPausePP(*(const MsgCmdPause*)c); break;
case CMD_RESUME: retval = new MsgCmdResumePP(*(const MsgCmdResume*)c); break;
case CMD_FOLLOW: retval = new MsgCmdFollowPP(*(const MsgCmdFollow*)c); break;
- //case CMD_LOAD: retval = new MsgCmdLoadPP(*(const MsgCmdLoad*)c); break;
- //case CMD_UNLOAD: retval = new MsgCmdUnloadPP(*(const MsgCmdUnload*)c); break;
case REQ_LOCATION: retval = new MsgReqLocationPP(*(const MsgReqLocation*)c); break;
case ROB_ACK: retval = new MsgRobAckPP(*(const MsgRobAck*)c); break;
case ROB_DONE: retval = new MsgRobDonePP(*(const MsgRobDone*)c); break;
@@ -132,10 +131,6 @@
if(resume != NULL) return resume->clone();
const MsgCmdFollowPP *follow = dynamic_cast<const MsgCmdFollowPP*>(this);
if(follow != NULL) return follow->clone();
- //const MsgCmdLoadPP *load = dynamic_cast<const MsgCmdLoadPP*>(this);
- //if(load != NULL) return load->clone();
- //const MsgCmdUnloadPP *unload = dynamic_cast<const MsgCmdUnloadPP*>(this);
- //if(unload != NULL) return unload->clone();
const MsgReqLocationPP *rloc = dynamic_cast<const MsgReqLocationPP*>(this);
if(rloc != NULL) return rloc->clone();
const MsgRobAckPP *ack = dynamic_cast<const MsgRobAckPP*>(this);
@@ -163,8 +158,6 @@
if(dynamic_cast<const MsgCmdResumePP*>(this) != NULL) return CMD_RESUME;
if(dynamic_cast<const MsgCmdFollowPP*>(this) != NULL) return CMD_FOLLOW;
if(dynamic_cast<const MsgCmdCoverPP*>(this) != NULL) return CMD_COVER;
- //if(dynamic_cast<const MsgCmdLoadPP*>(this) != NULL) return CMD_LOAD;
- //if(dynamic_cast<const MsgCmdUnloadPP*>(this) != NULL) return CMD_UNLOAD;
if(dynamic_cast<const MsgReqLocationPP*>(this) != NULL) return REQ_LOCATION;
if(dynamic_cast<const MsgRobAckPP*>(this) != NULL) return ROB_ACK;
if(dynamic_cast<const MsgRobDonePP*>(this) != NULL) return ROB_DONE;
@@ -179,22 +172,6 @@
return sender;
}
-/*
-void Msg::stamp(const string& hostname, const string& recipient) {
- MsgHeader *h = (MsgHeader*)this;
- //hostname.copy(h->sender, SADDR_LENGTH);
- //recipient.copy(h->recipient, SADDR_LENGTH);
-#ifdef WIN32
-#error fixme
-#else
- struct timeval tv;
- struct timezone tz;
- gettimeofday(&tv, &tz);
- h->tstamp = tv.tv_sec + (float)tv.tv_usec/1e6;
-#endif
-};
-*/
-
MsgCmdActionPP::MsgCmdActionPP() {}
MsgCmdActionPP::MsgCmdActionPP(const MsgCmdAction& c) : content_(c) {}
MsgHeader* MsgCmdActionPP::header() {return (MsgHeader*)&content_;};
@@ -205,11 +182,6 @@
return &content_;
}
-/*
-MsgCmdHaltPP::MsgCmdHaltPP()
- : MsgCmdActionPP(construct_MsgCmdAction("halt", iTaskCounter++)) {};
-*/
-
MsgCmdHaltPP::MsgCmdHaltPP() {
content_.type = CMD_HALT;
content_.len = sizeof(content_);
@@ -387,34 +359,6 @@
return out.str();
};
-/*
-MsgCmdLoadPP::MsgCmdLoadPP() {
- content_.type = CMD_LOAD;
- content_.len = sizeof(content_);
- content_.taskid = iTaskCounter++;
- content_.priority = 1;
-};
-MsgCmdLoadPP::MsgCmdLoadPP(const MsgCmdLoad& x) : content_(x) {};
-MsgCmdLoadPP* MsgCmdLoadPP::clone() const {return new MsgCmdLoadPP(*this);};
-MsgHeader* MsgCmdLoadPP::header() {return (MsgHeader*)&content_;};
-const MsgCmdLoad* MsgCmdLoadPP::content() const {return &content_;}
-string MsgCmdLoadPP::action_string() const {return "load";}
-string MsgCmdLoadPP::render() const {return "Load";};
-
-MsgCmdUnloadPP::MsgCmdUnloadPP() {
- content_.type = CMD_UNLOAD;
- content_.len = sizeof(content_);
- content_.taskid = iTaskCounter++;
- content_.priority = 1;
-};
-MsgCmdUnloadPP::MsgCmdUnloadPP(const MsgCmdUnload& x) : content_(x) {};
-MsgCmdUnloadPP* MsgCmdUnloadPP::clone() const {return new MsgCmdUnloadPP(*this);};
-MsgHeader* MsgCmdUnloadPP::header() {return (MsgHeader*)&content_;};
-const MsgCmdUnload* MsgCmdUnloadPP::content() const {return &content_;}
-string MsgCmdUnloadPP::action_string() const {return "unload";}
-string MsgCmdUnloadPP::render() const {return "Unload";};
-*/
-
MsgReqLocationPP::MsgReqLocationPP() {
content_.type = REQ_LOCATION;
content_.len = sizeof(content_);
@@ -609,6 +553,8 @@
return retval;
}
+// **************** SetPos ****************************************************
+
const char *const MsgCmdSetPos::scan_template = "setpos (%d %d) %d";
bool MsgCmdSetPos::parse() {
Modified: TeamTalk/Agents/TeamTalkBackend/backendstub/robot.cpp
===================================================================
--- TeamTalk/Agents/TeamTalkBackend/backendstub/robot.cpp 2006-11-08 20:05:26 UTC (rev 543)
+++ TeamTalk/Agents/TeamTalkBackend/backendstub/robot.cpp 2006-11-09 19:17:31 UTC (rev 544)
@@ -19,16 +19,16 @@
static unsigned char* image = NULL;
static int image_size;
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;
+ 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) {
@@ -43,10 +43,10 @@
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;
+ unsigned int result = 0;
+ for (unsigned int i = 0; s [i] != 0; ++i)
+ result = (result & mask) ^ (result << shift) ^ s [i];
+ return result;
}
const float Robot::velocity=1.0F; //velocity in meters per second
@@ -64,17 +64,14 @@
void Robot::simulate(void *thisp) {
Robot* me = (Robot*) thisp;
- //const string hashstuff(me->server_->getHandle());
- //srand((unsigned int)time(NULL)*Hash(hashstuff));
EnterCriticalSection(&me->CriticalSection);
for(;;) {
LeaveCriticalSection(&me->CriticalSection);
do {
const Boeing::MsgCmd *bmsg;
- //if(me->server_->isConnected()) cerr << '.';
if(me->server_->isConnected() && (bmsg = me->server_->getNextMessage())) {
Msg* msg = Msg::interpret(bmsg->buff);
- //cerr << "got message: " << msg->render() << endl;
+ cerr << "got message: " << msg->render() << endl;
me->callback(msg, NULL);
} else Sleep((int) (updatePeriod*1000));
} while(!TryEnterCriticalSection(&me->CriticalSection));
@@ -155,18 +152,18 @@
}
Robot::Robot(const map<string, Robot*> *robs, string name)
- : x(0), goal_x(0), y(0), goal_y(0), r(0), goal_r(0), action(WAITING), robots(robs) {
+: x(0), goal_x(0), y(0), goal_y(0), r(0), goal_r(0), action(WAITING), robots(robs) {
InitializeCriticalSection(&CriticalSection);
- if(!image) readImage("pic.jpg");
- server_ = new Boeing::RobotServer();
+ if(!image) readImage("pic.jpg");
+ server_ = new Boeing::RobotServer();
server_->open();
simulationThread_ = (HANDLE)_beginthread(simulate, 0, (void *) this);
}
Robot::Robot(const map<string, Robot*> *robs, string name, unsigned int port)
- : x(0), goal_x(0), y(0), goal_y(0), r(0), goal_r(0), action(WAITING), robots(robs) {
+: x(0), goal_x(0), y(0), goal_y(0), r(0), goal_r(0), action(WAITING), robots(robs) {
InitializeCriticalSection(&CriticalSection);
- if(!image) readImage("pic.jpg");
+ if(!image) readImage("pic.jpg");
server_ = new Boeing::RobotServer();
server_->open(port);
simulationThread_ = (HANDLE)_beginthread(simulate, 0, (void *) this);
@@ -188,55 +185,58 @@
} else {
temp_message = msgp->clone();
}
- if(const MsgMapReqPP* req_image = dynamic_cast<const MsgMapReqPP*>(temp_message)) {
- server_->sendJPEGImage(image, image_size, 320, 240, req_image->getInvoice());
- delete temp_message;
- return;
- }
+ if(const MsgMapReqPP* req_image = dynamic_cast<const MsgMapReqPP*>(temp_message)) {
+ server_->sendJPEGImage(image, image_size, 320, 240, req_image->getInvoice());
+ delete temp_message;
+ return;
+ }
if(const MsgReqLocationPP* rloc = dynamic_cast<const MsgReqLocationPP*>(temp_message)) {
server_->sendLocation(x, y, r, true);
- delete temp_message;
- return;
- }
+ delete temp_message;
+ return;
+ }
EnterCriticalSection(&CriticalSection);
if(const MsgCmdGoToRelativePP* goto_relative = dynamic_cast<const MsgCmdGoToRelativePP*>(temp_message)) {
- cout << "got: " << *goto_relative << endl;
+ cerr << "got: " << *goto_relative << endl;
float m_x = goto_relative->content()->x;
float m_y = goto_relative->content()->y;
float dir = r+atan2(m_y, m_x);
float len = sqrt(m_x*m_x+m_y*m_y);
setGoal(x+cos(dir)*len, y+sin(dir)*len); //will report done when complete
} else if(const MsgCmdHaltPP* halt = dynamic_cast<const MsgCmdHaltPP*>(temp_message)) {
- cout << "got: " << *halt << endl;
+ cerr << "got: " << *halt << endl;
action = WAITING;
cerr << "done with halt" << endl;
} else if(const MsgCmdGoToGlobalPP* goto_global = dynamic_cast<const MsgCmdGoToGlobalPP*>(temp_message)) {
- cout << "got: " << *goto_global << endl;
+ cerr << "got: " << *goto_global << endl;
setGoal(goto_global->content()->x, goto_global->content()->y);
} else if(const MsgCmdTurnRelativePP* turn = dynamic_cast<const MsgCmdTurnRelativePP*>(temp_message)) {
- cout << "got: " << *turn << endl;
+ cerr << "got: " << *turn << endl;
goal_r = canonical_angle(r+turn->content()->angle);
action = GO_TO_ANGLE; //will report done when complete
} else if(const MsgCmdHomePP* home = dynamic_cast<const MsgCmdHomePP*>(temp_message)) {
- cout << "got: " << *home << endl;
+ cerr << "got: " << *home << endl;
setGoal(0, 0);
} else if(const MsgCmdPausePP* pause = dynamic_cast<const MsgCmdPausePP*>(temp_message)) {
- cout << "got: " << *pause << endl;
+ cerr << "got: " << *pause << endl;
if(action != PAUSED) {
paused_action = action;
action = PAUSED;
}
cerr << "done with pause" << endl;
} else if(const MsgCmdResumePP* resume = dynamic_cast<const MsgCmdResumePP*>(temp_message)) {
- cout << "got: " << *resume << endl;
+ cerr << "got: " << *resume << endl;
if(action == PAUSED) {
action = paused_action;
cerr << "done with resume" << endl; //resume only succeeds if already paused
}
} else if(const MsgCmdFollowPP* follow = dynamic_cast<const MsgCmdFollowPP*>(temp_message)) {
- cout << "got: " << *follow << endl;
+ cerr << "got: " << *follow << endl;
//setFollow(follow->content()->leader);
- }
+ } else if(const MsgCmdSetPos* setpos = dynamic_cast<const MsgCmdSetPos*>(temp_message)) {
+ cerr << "got: " << *setpos << endl;
+ x = setpos->getX(); y = setpos->getY(); r = setpos->getAngle();
+ }
LeaveCriticalSection(&CriticalSection);
delete temp_message;
}
Modified: TeamTalk/Agents/TeamTalkBackend/gal_be.cpp
===================================================================
--- TeamTalk/Agents/TeamTalkBackend/gal_be.cpp 2006-11-08 20:05:26 UTC (rev 543)
+++ TeamTalk/Agents/TeamTalkBackend/gal_be.cpp 2006-11-09 19:17:31 UTC (rev 544)
@@ -74,27 +74,30 @@
vector<GalIO_CommStruct*> comms;
GalIO_CommStruct* skeleton_comm = NULL;
-void writeFrameAllHubs(Gal_Frame f) {
- GalUtil_Debug1("writing to all hubs\n");
+void writeFrameAllHubs(Gal_Frame f)
+{
+ GalUtil_Debug1("writing to all hubs\n");
for(vector<GalIO_CommStruct*>::iterator i=comms.begin(); i!=comms.end(); i++) {
- GalUtil_Debug1("writing to hub\n");
+ GalUtil_Debug1("writing to hub\n");
GalIO_CommWriteFrame(*i, f, GAL_FALSE);
}
};
-void writeFrameFirstHub(Gal_Frame f) {
- for(int i=0; i<30; i++) {
- if(skeleton_comm) {
- GalIO_CommWriteFrame(skeleton_comm, f, GAL_FALSE);
- return;
- }
- GalUtil_Debug1("skeleton unavailable\n");
- Sleep(1000);
- }
+void writeFrameFirstHub(Gal_Frame f)
+{
+ for(int i=0; i<30; i++) {
+ if(skeleton_comm) {
+ GalIO_CommWriteFrame(skeleton_comm, f, GAL_FALSE);
+ return;
+ }
+ GalUtil_Debug1("skeleton unavailable\n");
+ Sleep(1000);
+ }
};
// [2006-07-14] (tk): this restarts sphinx for a newly created dict and lm
-void restartDecoder() {
+void restartDecoder()
+{
cerr << "sending restart_decoder" << endl;
Gal_Frame gfInput = Gal_MakeFrame("restart_decoder", GAL_CLAUSE);
writeFrameAllHubs(gfInput);
@@ -104,7 +107,8 @@
// message towards the DM: The message will be
// emulating a phoenix parse, of the form
// RobotMessage [RobotMessage] ( [MsgType] ( Message ) )
-void SendMessageToDM(string sMsgType, string sMessage) {
+void SendMessageToDM(string sMsgType, string sMessage)
+{
// variables holding the nested galaxy parse frame
Gal_Frame gfInput, gfParse, gfSlot, gfNet;
Gal_Object *pgoParses, *pgoSlots, *pgoNets;
@@ -169,7 +173,8 @@
//-------------------------------------------------------------------
// [2006-07-22] (tk): send a robot_config message to galaxy hubs
//-------------------------------------------------------------------
-static void SendRobotConfigMessage(const string& sName, const string& sVoice) {
+static void SendRobotConfigMessage(const string& sName, const string& sVoice)
+{
// variables holding the nested galaxy parse frame
Gal_Frame f = Gal_MakeFrame("robot_config", GAL_CLAUSE);
@@ -178,7 +183,8 @@
writeFrameAllHubs(f);
}
-static void stubMessage(const Msg* msgp) {
+static void stubMessage(const Msg* msgp)
+{
const MsgRobLocationPP *rloc;
if((rloc = dynamic_cast<const MsgRobLocationPP*>(msgp)) != NULL) {
map<string, Boeing::MsgRobLocation>::iterator i = locations.find(rloc->getSender());
@@ -194,7 +200,8 @@
}
}
-template<class Iter> string string_space(const Iter begin, const Iter end) {
+template<class Iter> string string_space(const Iter begin, const Iter end)
+{
ostringstream out;
if(begin == end) return out.str();
for(Iter i = begin;;) {
@@ -206,7 +213,8 @@
return out.str();
}
-template<class String_Array_Elem> string string_space(const String_Array_Elem** begin) {
+template<class String_Array_Elem> string string_space(const String_Array_Elem** begin)
+{
ostringstream out;
if(*begin == NULL) return out.str();
for(const String_Array_Elem** i = begin;;) {
@@ -218,7 +226,8 @@
return out.str();
}
-BOOL WINAPI ConsoleHandler(DWORD CEvent) {
+BOOL WINAPI ConsoleHandler(DWORD CEvent)
+{
//kill all of the spawned children
while(!children.empty()) {
HANDLE child = children.top();
@@ -229,7 +238,8 @@
return TRUE;
}
-static PROCESS_INFORMATION spawnProg(const string& title, const string& dir, const string& exe, const string& args) {
+static PROCESS_INFORMATION spawnProg(const string& title, const string& dir, const string& exe, const string& args)
+{
//TK: CreateProcess wants a writable char* for some stupid reason
ostringstream cmdline;
cmdline << exe << ' ' << args;
@@ -268,22 +278,25 @@
return pi;
}
-void substitute(string& temp, const string& var, const string& val) {
+void substitute(string& temp, const string& var, const string& val)
+{
for(string::size_type j = temp.find(var); j != string::npos; j = temp.find(var)) {
temp.replace(j, var.size(), val);
}
}
-void substitute(string& temp, const map<string, string>& subs) {
+void substitute(string& temp, const map<string, string>& subs)
+{
for(map<string, string>::const_iterator i = subs.begin(); i != subs.end(); i++) {
substitute(temp, i->first, i->second);
}
}
static string writeSpecializedConfig(const string temp,
- const string ext,
- const map<string, string>& subs,
- const string name = string()) {
+ const string ext,
+ const map<string, string>& subs,
+ const string name = string())
+{
ostringstream cfilename;
cfilename << temp;
if(!name.empty()) cfilename << '-' << name;
@@ -309,13 +322,15 @@
return cfilename.str();
}
-template<class C> string stringOf(const C& x) {
+template<class C> string stringOf(const C& x)
+{
ostringstream oss;
oss << x;
return oss.str();
}
-static void spawnHub(const string& uppername) {
+static void spawnHub(const string& uppername)
+{
map<string, string> subs;
subs["%%DialogManager%%"] = uppername;
subs["%%DMPort%%"] = stringOf(iDMPort);
@@ -328,7 +343,8 @@
spawnProg(cfilename, ".", HUB, args.str());
}
-static void spawnHelios(const string& uppername) {
+static void spawnHelios(const string& uppername)
+{
map<string, string> subs;
subs["%%NAME%%"] = uppername;
@@ -339,7 +355,8 @@
spawnProg(cfilename, ".", HELIOS, args.str());
}
-static void spawnDM(const string& uppername, const string& safeness) {
+static void spawnDM(const string& uppername, const string& safeness)
+{
map<string, string> subs;
subs["%%ServerName%%"] = uppername;
subs["%%ServerPort%%"] = stringOf(iDMPort);
@@ -364,7 +381,8 @@
spawnProg(cfilename, ".", DM, args.str());
}
-static void spawnMinorGalaxy(const string& name, const string& safeness) {
+static void spawnMinorGalaxy(const string& name, const string& safeness)
+{
string uppername(name);
string uppersafeness(safeness);
toupper(uppername); toupper(uppersafeness);
@@ -376,7 +394,8 @@
iDMPort++; iHeliosPort++;
}
-static void addRobotNamesToGrammar(const vector<string>& names) {
+static void addRobotNamesToGrammar(const vector<string>& names)
+{
map<string, string> subs;
ostringstream gname;
for(vector<string>::const_iterator i = names.begin(); i != names.end(); i++) {
@@ -389,7 +408,8 @@
WaitForSingleObject(lm_build_proc.hProcess, INFINITE);
}
-static void addRobotVoices() {
+static void addRobotVoices()
+{
vector<string> names = p_client->getClientList();
for(vector<string>::const_iterator i = names.begin(); i != names.end(); i++) {
string name(toupper(*i));
@@ -443,39 +463,41 @@
* assured that that was not nor would be the
* case
*/
-static int complete_frame(char c) {
+static int complete_frame(char c)
+{
switch (c) {
- case '\n':
- /* Test if the unclosed paren count is zero and
- we have seen one or more open parens (to account
- for any leading newlines) */
+ case '\n':
+ /* Test if the unclosed paren count is zero and
+ we have seen one or more open parens (to account
+ for any leading newlines) */
- if ((unclosed == 0) && opened)
- return TRUE;
- else
- return FALSE;
+ if ((unclosed == 0) && opened)
+ return TRUE;
+ else
+ return FALSE;
- case '{':
- unclosed++;
- opened = TRUE;
+ case '{':
+ unclosed++;
+ opened = TRUE;
- return FALSE;
+ return FALSE;
- case '}':
- unclosed--;
+ case '}':
+ unclosed--;
- if (unclosed < 0) {
- GalUtil_Fatal("Unmatched close parens seen in frame\n");
- }
+ if (unclosed < 0) {
+ GalUtil_Fatal("Unmatched close parens seen in frame\n");
+ }
- return FALSE;
+ return FALSE;
- default:
- return FALSE;
+ default:
+ return FALSE;
}
}
-void fCallback(const string& x, const string& y) {
+void fCallback(const string& x, const string& y)
+{
char *s = "{c FromDialogue :output_frame {c greeting } :is_greeting 1 }";
Gal_Frame greeting = Gal_ReadFrameFromString(s);
@@ -483,8 +505,9 @@
Gal_FreeFrame(greeting);
}
-void trackbots(void *nul) {
- cerr << "starting trackbots thread" << endl;
+void trackbots(void *nul)
+{
+ cerr << "starting trackbots thread" << endl;
for(int t=0; true; t++) {
Sleep(1000);
vector<string> bots = p_client->getClientList();
@@ -494,157 +517,161 @@
cerr << "can't find robot: " << *i << endl;
continue;
}
- cerr << "sending location request" << endl;
+ cerr << "sending location request" << endl;
r->sendReqLocation();
//p_client->sendPacket(*i, MsgReqLocationPP());
}
- if(!(t%3)) { //send map keepalive every 3 seconds
- m_client->sendKeepAlive(0);
- }
- if(!(t%40)) { //send image request every 40 seconds
- if(!bots.empty()) {
- Boeing::RobotClient* r = p_client->find("FESTO");
- if(r == NULL) {
- cerr << "can't find robot: " << bots.front() << endl;
- } else {
- r->sendReqImage();
- }
- }
- }
+ if(!(t%3)) { //send map keepalive every 3 seconds
+ m_client->sendKeepAlive(0);
+ }
+ if(!(t%40)) { //send image request every 40 seconds
+ if(!bots.empty()) {
+ Boeing::RobotClient* r = p_client->find("FESTO");
+ if(r == NULL) {
+ cerr << "can't find robot: " << bots.front() << endl;
+ } else {
+ r->sendReqImage();
+ }
+ }
+ }
}
}
-static void traderlistener(void *nul) {
- for(;;) {
- GalUtil_Warn("waiting for next trader message\n");
- const Boeing::MsgTraderClient* msg = t_client->getNextMessage();
- if(!msg) {
- GalUtil_Warn("msgtraderclient is null\n");
- Sleep(500);
- continue;
- }
- Gal_Frame f = Gal_MakeFrame("trader_message", GAL_CLAUSE);
- switch(msg->hdr.type) {
- case Boeing::INFO:
- Gal_SetProp(f, ":type", Gal_StringObject("info"));
- break;
- default:
- GalUtil_Warn("unknown message type '%d' in traderlistener", msg->hdr.type);
- Gal_FreeFrame(f);
- continue;
- }
- GalUtil_Warn("got info message: %s\n", msg->info.task);
- istringstream input(msg->info.task);
- string token;
- string ip;
- string object;
- float x, y;
- input >> token;
- GalUtil_Debug1("got info token %s\n", token.c_str());
- if(token != "object") {
- ip = token;
- input >> token;
- if(token != "object") {
- GalUtil_Debug1("was expecting 'object'\n");
- continue;
- }
- }
- input >> object;
- GalUtil_Debug1("got info object %s\n", object.c_str());
- input >> token;
- GalUtil_Debug1("got info token %s\n", token.c_str());
- if(token != "at") {
- GalUtil_Debug1("was expecting 'at'\n");
- continue;
- }
- input >> token;
- GalUtil_Debug1("got info token %s\n", token.c_str());
- if(token.empty() || token.at(0) != '(') {
- GalUtil_Debug1("was expecting (...\n");
- continue;
- }
- if(token == "(") {
- input >> token;
- GalUtil_Debug1("got info token %s\n", token.c_str());
- x = (float)atof(token.c_str());
- } else {
- x = (float)atof(token.c_str()+1);
- }
- GalUtil_Debug1("got info x: %f\n", x);
- input >> token;
- GalUtil_Debug1("got info token %s\n", token.c_str());
- if(token.empty()) {
- GalUtil_Debug1("was expecting ...)\n");
- continue;
- } else if(token.at(token.length()-1) != ')') {
- token.resize(token.length()-1);
- }
- y = (float)atof(token.c_str());
- GalUtil_Debug1("got info y: %f\n", y);
- Gal_SetProp(f, ":ip", Gal_StringObject(ip.c_str()));
- Gal_SetProp(f, ":object", Gal_StringObject(object.c_str()));
- Gal_SetProp(f, ":x", Gal_FloatObject(x));
- Gal_SetProp(f, ":y", Gal_FloatObject(y));
- writeFrameFirstHub(f);
- Gal_FreeFrame(f);
- }
+static void traderlistener(void *nul)
+{
+ for(;;) {
+ GalUtil_Warn("waiting for next trader message\n");
+ const Boeing::MsgTraderClient* msg = t_client->getNextMessage();
+ if(!msg) {
+ GalUtil_Warn("msgtraderclient is null\n");
+ Sleep(500);
+ continue;
+ }
+ Gal_Frame f = Gal_MakeFrame("trader_message", GAL_CLAUSE);
+ switch(msg->hdr.type) {
+ case Boeing::INFO:
+ Gal_SetProp(f, ":type", Gal_StringObject("info"));
+ break;
+ default:
+ GalUtil_Warn("unknown message type '%d' in traderlistener", msg->hdr.type);
+ Gal_FreeFrame(f);
+ continue;
+ }
+ GalUtil_Warn("got info message: %s\n", msg->info.task);
+ istringstream input(msg->info.task);
+ string token;
+ string ip;
+ string object;
+ float x, y;
+ input >> token;
+ GalUtil_Debug1("got info token %s\n", token.c_str());
+ if(token != "object") {
+ ip = token;
+ input >> token;
+ if(token != "object") {
+ GalUtil_Debug1("was expecting 'object'\n");
+ continue;
+ }
+ }
+ input >> object;
+ GalUtil_Debug1("got info object %s\n", object.c_str());
+ input >> token;
+ GalUtil_Debug1("got info token %s\n", token.c_str());
+ if(token != "at") {
+ GalUtil_Debug1("was expecting 'at'\n");
+ continue;
+ }
+ input >> token;
+ GalUtil_Debug1("got info token %s\n", token.c_str());
+ if(token.empty() || token.at(0) != '(') {
+ GalUtil_Debug1("was expecting (...\n");
+ continue;
+ }
+ if(token == "(") {
+ input >> token;
+ GalUtil_Debug1("got info token %s\n", token.c_str());
+ x = (float)atof(token.c_str());
+ } else {
+ x = (float)atof(token.c_str()+1);
+ }
+ GalUtil_Debug1("got info x: %f\n", x);
+ input >> token;
+ GalUtil_Debug1("got info token %s\n", token.c_str());
+ if(token.empty()) {
+ GalUtil_Debug1("was expecting ...)\n");
+ continue;
+ } else if(token.at(token.length()-1) != ')') {
+ token.resize(token.length()-1);
+ }
+ y = (float)atof(token.c_str());
+ GalUtil_Debug1("got info y: %f\n", y);
+ Gal_SetProp(f, ":ip", Gal_StringObject(ip.c_str()));
+ Gal_SetProp(f, ":object", Gal_StringObject(object.c_str()));
+ Gal_SetProp(f, ":x", Gal_FloatObject(x));
+ Gal_SetProp(f, ":y", Gal_FloatObject(y));
+ writeFrameFirstHub(f);
+ Gal_FreeFrame(f);
+ }
}
-static void maplistener(void *nul) {
- for(;;) {
- GalUtil_Debug1("waiting for next map message\n");
- const Boeing::MsgMapClient* msg = m_client->getNextMessage();
- if(!msg) {
- GalUtil_Debug1("msgmapclient is null\n");
- Sleep(1000);
- continue;
- }
- Gal_Frame f = Gal_MakeFrame("map_message", GAL_CLAUSE);
- switch(msg->hdr.type) {
- case Boeing::MAP_FULL:
- Gal_SetProp(f, ":type", Gal_StringObject("full"));
- break;
- case Boeing::MAP_DIFF:
- Gal_SetProp(f, ":type", Gal_StringObject("diff"));
- break;
- default:
- GalUtil_Fatal("unknown message type '%d' in maplistener", msg->hdr.type);
- Gal_FreeFrame(f);
- continue;
- }
- Gal_SetProp(f, ":x_size", Gal_IntObject(msg->map.x));
- Gal_SetProp(f, ":y_size", Gal_IntObject(msg->map.y));
- Gal_SetProp(f, ":resolution", Gal_IntObject(msg->map.resolution));
- int *temp = new int[msg->map.array_length];
- for(int i=0; i<msg->map.array_length; i++) temp[i] = msg->map.map[i];
- cerr << "got raw: " << temp[0]
- << " rl: " << ((temp[0]&0xFFFFFF00)>>8)
- << " rv: " << (temp[0]&0x000000FF) << endl;
- Gal_SetProp(f, ":encoded_map",
- Gal_CreateInt32Object((void *) temp, msg->map.array_length, 1));
- GalUtil_Debug1("sending map to hub\n");
- writeFrameFirstHub(f);
- Gal_FreeFrame(f);
- }
+static void maplistener(void *nul)
+{
+ for(;;) {
+ GalUtil_Debug1("waiting for next map message\n");
+ const Boeing::MsgMapClient* msg = m_client->getNextMessage();
+ if(!msg) {
+ GalUtil_Debug1("msgmapclient is null\n");
+ Sleep(1000);
+ continue;
+ }
+ Gal_Frame f = Gal_MakeFrame("map_message", GAL_CLAUSE);
+ switch(msg->hdr.type) {
+ case Boeing::MAP_FULL:
+ Gal_SetProp(f, ":type", Gal_StringObject("full"));
+ break;
+ case Boeing::MAP_DIFF:
+ Gal_SetProp(f, ":type", Gal_StringObject("diff"));
+ break;
+ default:
+ GalUtil_Fatal("unknown message type '%d' in maplistener", msg->hdr.type);
+ Gal_FreeFrame(f);
+ continue;
+ }
+ Gal_SetProp(f, ":x_size", Gal_IntObject(msg->map.x));
+ Gal_SetProp(f, ":y_size", Gal_IntObject(msg->map.y));
+ Gal_SetProp(f, ":resolution", Gal_IntObject(msg->map.resolution));
+ int *temp = new int[msg->map.array_length];
+ for(int i=0; i<msg->map.array_length; i++) temp[i] = msg->map.map[i];
+ cerr << "got raw: " << temp[0]
+ << " rl: " << ((temp[0]&0xFFFFFF00)>>8)
+ << " rv: " << (temp[0]&0x000000FF) << endl;
+ Gal_SetProp(f, ":encoded_map",
+ Gal_CreateInt32Object((void *) temp, msg->map.array_length, 1));
+ GalUtil_Debug1("sending map to hub\n");
+ writeFrameFirstHub(f);
+ Gal_FreeFrame(f);
+ }
}
static bool lastConfig = false;
-static bool testLastConfig(const string& source, const string& target) {
- //return true if target is newer than source
- struct _stat source_stat, target_stat;
- if(_stat(source.c_str(), &source_stat)) {
- cerr << "problem stating source: " << source << endl;
- return false;
- }
- if(_stat(target.c_str(), &target_stat)) {
- cerr << "problem stating target: " << target << endl;
- return false;
- }
- return target_stat.st_mtime > source_stat.st_mtime;
+static bool testLastConfig(const string& source, const string& target)
+{
+ //return true if target is newer than source
+ struct _stat source_stat, target_stat;
+ if(_stat(source.c_str(), &source_stat)) {
+ cerr << "problem stating source: " << source << endl;
+ return false;
+ }
+ if(_stat(target.c_str(), &target_stat)) {
+ cerr << "problem stating target: " << target << endl;
+ return false;
+ }
+ return target_stat.st_mtime > source_stat.st_mtime;
}
-void *init_server(Gal_Server *s, int argc, char **argv) {
+void *init_server(Gal_Server *s, int argc, char **argv)
+{
if (SetConsoleCtrlHandler((PHANDLER_ROUTINE)ConsoleHandler,TRUE)==FALSE) {
cerr << "Unable to install handler!" << endl;
return (void *) s;
@@ -671,26 +698,26 @@
try {
p_client = new TeamTalk::RobotClient("tester", &stubMessage, &spawnMinorGalaxy);
t_client = new Boeing::TraderClient();
- m_client = new Boeing::MapClient();
+ m_client = new Boeing::MapClient();
p_client->addRobotsFile("peerfile.txt", t_client, m_client);
- if(!(lastConfig = testLastConfig("peerfile.txt", "..\\..\\Resources\\DecoderConfig\\LanguageModel\\zap2LM.arpa")))
+ if(!(lastConfig = testLastConfig("peerfile.txt", "..\\..\\Resources\\DecoderConfig\\LanguageModel\\zap2LM.arpa")))
addRobotNamesToGrammar(p_client->getClientList());
- if(!t_client->isConnected()) {
- GalUtil_Fatal("optraderserver not found");
- } else {
- GalUtil_Debug1("connected to trader\n");
- }
- _beginthread(traderlistener, 0, (void*) NULL);
+ if(!t_client->isConnected()) {
+ GalUtil_Fatal("optraderserver not found");
+ } else {
+ GalUtil_Debug1("connected to trader\n");
+ }
+ _beginthread(traderlistener, 0, (void*) NULL);
- if(!m_client->isConnected()) {
- GalUtil_Fatal("mapserver not found");
- } else {
- if(!m_client->sendSubscribe(0))
- GalUtil_Fatal("couldn't send subscribe message to map server");
- }
- _beginthread(maplistener, 0, (void*) NULL);
+ if(!m_client->isConnected()) {
+ GalUtil_Fatal("mapserver not found");
+ } else {
+ if(!m_client->sendSubscribe(0))
+ GalUtil_Fatal("couldn't send subscribe message to map server");
+ }
+ _beginthread(maplistener, 0, (void*) NULL);
//start robot tracking thread
_beginthread(trackbots, 0, (void*) NULL);
@@ -704,9 +731,9 @@
Gal_Frame reinitialize(Gal_Frame f, void *server_data)
{
comms.push_back(GalSS_EnvComm((GalSS_Environment *)server_data));
- const char *user_id = Gal_GetString(f, ":user_id");
- if(!user_id) GalUtil_Debug1("no user_id\n");
- else if(!strcmp(user_id, "TeamTalk")) skeleton_comm = comms.back();
+ const char *user_id = Gal_GetString(f, ":user_id");
+ if(!user_id) GalUtil_Debug1("no user_id\n");
+ else if(!strcmp(user_id, "TeamTalk")) skeleton_comm = comms.back();
restartDecoder();
@@ -715,7 +742,8 @@
return f;
}
-int tkGetNumber999(const string& x) {
+int tkGetNumber999(const string& x)
+{
istringstream issX(x);
int retval = 0;
for(string token; issX >> token;) {
@@ -782,426 +810,467 @@
return retval;
}
-template<class A> bool sendAction(Boeing::RobotClient *r, const A *action) {
+template<class A> bool sendAction(Boeing::RobotClient *r, const A *action)
+{
return r->sendAction(action->content()->priority, action->content()->taskid, action->action_string().c_str());
}
-Gal_Frame launch_query(Gal_Frame f, void *server_data) {
- Gal_Object aStr;
- aStr = Gal_GetObject(f, ":inframe");
- if (aStr) {
+const char *const compass_points[] = {
+ "east", "east north east", "north east", "north north east",
+ "north", "north north west", "north west", "west north west",
+ "west", "west south west", "south west", "south south west",
+ "south", "south south east", "south east", "east south east"};
- //inframe = Gal_StringValue(aStr);
- GalUtil_Error("about to print a frame");
- Gal_PrFrame(f);
- char* s_inframe = Gal_StringValue(aStr);
- if(s_inframe) {
- //frame fixup -> ravenclaw doesn't make proper frames
- ostringstream fixed;
- string notfixed(s_inframe + 1);
- istringstream inotfixed(notfixed);
- fixed << "{c zap ";
- string token;
- do {
- inotfixed >> token;
- if(token == "}") break;
- fixed << ':' << token << ' ';
- char cpVal[255];
- inotfixed.ignore();
- inotfixed.getline(cpVal, 254);
- fixed << '"' << cpVal << "\" ";
- } while(inotfixed);
- GalUtil_Error("working with fixed version: %s", fixed.str().c_str());
- Gal_Frame inframe = Gal_ReadFrameFromString(fixed.str().c_str());
- if(inframe) {
- Gal_PrFrame(inframe);
- char* query = Gal_GetString(inframe, ":query");
- char* robot = Gal_GetString(inframe, ":robot_name");
- if(query) {
- GalUtil_Error("query is %s", query);
- if(robot) {
- GalUtil_Error("robot is %s", robot);
- if(!strcmp(query, "location_query")) {
- static const float precision = 1.1F; //110 cm precision
- ostringstream ossMessage;
- map<string, Boeing::MsgRobLocation>::iterator i = locations.find(robot);
- if(i == locations.end()) { //no location info yet?
- ossMessage << "I don't know." << endl;
- } else {
- const Vector p(i->second.x, i->second.y);
- double distanceFromHome = p.length();
- if(distanceFromHome < precision) {
- ossMessage << "I am home";
- } else {
- ossMessage << setiosflags(ios_base::fixed) << setprecision(1);
- ossMessage << "I am "
- // << "at location " << p.x << ' ' << p.y << ". "
- << p.length() << " meters from home; bearing is ";
- int a = ((int)(p.angle()*180/PI+PI/16)/16)%16;
- switch(a) {
- case 0: ossMessage << "east."; break;
- case 1: ossMessage << "east north east."; break;
- case 2: ossMessage << "north east."; break;
- case 3: ossMessage << "north north east."; break;
- case 4: ossMessage << "north."; break;
- case 5: ossMessage << "north north west."; break;
- case 6: ossMessage << "north west."; break;
- case 7: ossMessage << "west north west."; break;
- case 8: ossMessage << "west."; break;
- case 9: ossMessage << "west south west."; break;
- case 10: ossMessage << "south west"; break;
- case 11: ossMessage << "south south west."; break;
- case 12: ossMessage << "south."; break;
- case 13: ossMessage << "south south east."; break;
- case 14: ossMessage << "south east."; break;
- case 15: ossMessage << "east south east."; break;
- default: ossMessage << "unknown.";
- }
- //<< p.angle()*180/PI << " degrees to the left.";
- }
- }
+static void location_query(const char* robot)
+{
+ static const float precision = 1.1F; //110 cm precision
+ ostringstream ossMessage;
+ map<string, Boeing::MsgRobLocation>::iterator i = locations.find(robot);
+ if(i == locations.end()) { //no location info yet?
+ ossMessage << "I don't know." << endl;
+ } else {
+ const Vector p(i->second.x, i->second.y);
+ double distanceFromHome = p.length();
+ if(distanceFromHome < precision) {
+ ossMessage << "I am home";
+ } else {
+ ossMessage << setiosflags(ios_base::fixed) << setprecision(1);
+ ossMessage << "I am "
+ << p.length() << " meters from home; bearing is ";
+ int a = ((int)(p.angle()*180/PI+PI/16)/16)%16;
+ ossMessage << compass_points[a] << '.';
+/*
+ switch(a)
+ {
+ case 0: ossMessage << "east."; break; }
+ case 1: ossMessage << "east north east."; break;
+ case 2: ossMessage << "north east."; break;
+ case 3: ossMessage << "north north east."; break;
+ case 4: ossMessage << "north."; break;
+ case 5: ossMessage << "north north west."; break;
+ case 6: ossMessage << "north west."; break;
+ case 7: ossMessage << "west north west."; break;
+ case 8: ossMessage << "west."; break;
+ case 9: ossMessage << "west south west."; break;
+ case 10: ossMessage << "south west"; break;
+ case 11: ossMessage << "south south west."; break;
+ case 12: ossMessage << "south."; break;
+ case 13: ossMessage << "south south east."; break;
+ case 14: ossMessage << "south east."; break;
+ case 15: ossMessage << "east south east."; break;
+ default: ossMessage << "unknown."; break;
+ }
+ */
+ }
+ }
- // [2005-09-18] (dbohus): send the message through the hub towards
- // helios and the DM
- SendMessageToDM("Location", ossMessage.str());
- }
- else if(!strcmp(query, "move_cardinal_command")) {
- GalUtil_Error("sending move query to backend");
- string s_distance(Gal_GetString(inframe, ":distance"));
- string s_units(Gal_GetString(inframe, ":units"));
- string s_direction(Gal_GetString(inframe, ":direction"));
- if(!(s_distance.empty() && s_direction.empty())) {
- GalUtil_Error("distance or direction missing");
- } else {
- Point vec(static_cast<float>(tkGetNumber999(s_distance)), 0);
- if(!s_distance.empty()) {
- if(!s_units.empty()) {
- if(s_units == "FOOT" || s_units == "FEET") {
- vec.x *= 0.3048F;
- } else if(s_units == "YARD" || s_units == "YARDS") {
- vec.x *= 0.9144F;
- }
- }
- MsgCmdGoToRelativePP *go = NULL;
- if(s_direction == "NORTH")
- go = new MsgCmdGoToRelativePP(vec.rotate(PI/2));
- else if(s_direction == "NORTHEAST")
- go = new MsgCmdGoToRelativePP(vec.rotate(PI/4));
- else if(s_direction == "NORTHWEST")
- go = new MsgCmdGoToRelativePP(vec.rotate(3*PI/4));
- else if(s_direction == "EAST")
- go = new MsgCmdGoToRelativePP(vec);
- else if(s_direction == "SOUTH")
- go = new MsgCmdGoToRelativePP(vec.rotate(-PI/2));
- else if(s_direction == "SOUTHEAST")
- go = new MsgCmdGoToRelativePP(vec.rotate(-PI/4));
- else if(s_direction == "SOUTHWEST")
- go = new MsgCmdGoToRelativePP(vec.rotate(-3*PI/4));
- else if(s_direction == "WEST")
- go = new MsgCmdGoToRelativePP(vec.rotate(PI));
- if(go != NULL) {
- Boeing::RobotClient *r = p_client->find(robot);
- if(r)
+ // [2005-09-18] (dbohus): send the message through the hub towards
+ // helios and the DM
+ SendMessageToDM("Location", ossMessage.str());
+}
+
+static void move_cardinal_command(const char* robot, const Gal_Frame& inframe)
+{
+ GalUtil_Error("sending move query to backend");
+ string s_distance(Gal_GetString(inframe, ":distance"));
+ string s_units(Gal_GetString(inframe, ":units"));
+ string s_direction(Gal_GetString(inframe, ":direction"));
+ if(!(s_distance.empty() && s_direction.empty())) {
+ GalUtil_Error("distance or direction missing");
+ } else {
+ Point vec(static_cast<float>(tkGetNumber999(s_distance)), 0);
+ if(!s_distance.empty()) {
+ if(!s_units.empty()) {
+ if(s_units == "FOOT" || s_units == "FEET") {
+ vec.x *= 0.3048F;
+ } else if(s_units == "YARD" || s_units == "YARDS") {
+ vec.x *= 0.9144F;
+ }
+ }
+ MsgCmdGoToRelativePP *go = NULL;
+ if(s_direction == "NORTH")
+ go = new MsgCmdGoToRelativePP(vec.rotate(PI/2));
+ else if(s_direction == "NORTHEAST")
+ go = new MsgCmdGoToRelativePP(vec.rotate(PI/4));
+ else if(s_direction == "NORTHWEST")
+ go = new MsgCmdGoToRelativePP(vec.rotate(3*PI/4));
+ else if(s_direction == "EAST")
+ go = new MsgCmdGoToRelativePP(vec);
+ else if(s_direction == "SOUTH")
+ go = new MsgCmdGoToRelativePP(vec.rotate(-PI/2));
+ else if(s_direction == "SOUTHEAST")
+ go = new MsgCmdGoToRelativePP(vec.rotate(-PI/4));
+ else if(s_direction == "SOUTHWEST")
+ go = new MsgCmdGoToRelativePP(vec.rotate(-3*PI/4));
+ else if(s_direction == "WEST")
+ go = new MsgCmdGoToRelativePP(vec.rotate(PI));
+ if(go != NULL) {
+ Boeing::RobotClient *r = p_client->find(robot);
+ if(r)
#ifdef USE_TXT_COMMANDS
- sendAction(r, go);
+ sendAction(r, go);
#else
- r->sendGoToCmd(1, go->content()->taskid,
- go->content()->x, go->content()->y, go->content()->angle,
- go->content()->useAngle(), go->content()->useRelative());
+ r->sendGoToCmd(1, go->content()->taskid,
+ go->content()->x, go->content()->y, go->content()->angle,
+ go->content()->useAngle(), go->content()->useRelative());
#endif
- delete go;
- } else
- GalUtil_Error("unknown direction %s", s_direction.c_str());
- }
- }
- }
- else if(!strcmp(query, "move_relative_command")) {
- GalUtil_Error("sending move relative query to backend");
- string s_distance;
- if (Gal_GetString(inframe, ":distance"))
- s_distance = Gal_GetString(inframe, ":distance");
- string s_direction;
- if (Gal_GetString(inframe, ":direction"))
- s_direction = Gal_GetString(inframe, ":direction");
- string s_units;
- if (Gal_GetString(inframe, ":units"))
- s_units = Gal_GetString(inframe, ":units");
- if(s_distance.empty() || s_direction.empty()) {
- GalUtil_Error("distance or direction missing");
- } else {
- if(!s_distance.empty()) {
- Point vec(static_cast<float>(tkGetNumber999(s_distance)), 0);
- if(!s_units.empty()) {
- if(s_units == "FOOT" || s_units == "FEET") {
- vec.x *= 0.3048F;
- } else if(s_units == "YARD" || s_units == "YARDS") {
- vec.x *= 0.9144F;
- }
- }
- MsgCmdGoToRelativePP *go = NULL;
- if(s_direction == "STRAIGHT" ||
- s_direction == "FORWARD" ||
- s_direction == "FORWARDS" ||
- s_direction == "AROUND")
- go = new MsgCmdGoToRelativePP(vec);
- else if(s_direction.substr(0, 5) == "RIGHT")
- go = new MsgCmdGoToRelativePP(vec.rotate(-PI/2));
- else if(s_direction.substr(0, 4) == "LEFT")
- go = new MsgCmdGoToRelativePP(vec.rotate(PI/2));
- else if(s_direction == "BACK" ||
- s_direction == "BACKWARD" ||
- s_direction == "BACKWARDS")
- go = new MsgCmdGoToRelativePP(vec.rotate(PI));
- if(go != NULL) {
- Boeing::RobotClient *r = p_client->find(robot);
- if(r)
+ delete go;
+ } else
+ GalUtil_Error("unknown direction %s", s_direction.c_str());
+ }
+ }
+}
+
+static void move_relative_command(const char* robot, const Gal_Frame& inframe)
+{
+ GalUtil_Error("sending move relative query to backend");
+ string s_distance;
+ if (Gal_GetString(inframe, ":distance"))
+ s_distance = Gal_GetString(inframe, ":distance");
+ string s_direction;
+ if (Gal_GetString(inframe, ":direction"))
+ s_direction = Gal_GetString(inframe, ":direction");
+ string s_units;
+ if (Gal_GetString(inframe, ":units"))
+ s_units = Gal_GetString(inframe, ":units");
+ if(s_distance.empty() || s_direction.empty()) {
+ GalUtil_Error("distance or direction missing");
+ } else {
+ if(!s_distance.empty()) {
+ Point vec(static_cast<float>(tkGetNumber999(s_distance)), 0);
+ if(!s_units.empty()) {
+ if(s_units == "FOOT" || s_units == "FEET") {
+ vec.x *= 0.3048F;
+ } else if(s_units == "YARD" || s_units == "YARDS") {
+ vec.x *= 0.9144F;
+ }
+ }
+ MsgCmdGoToRelativePP *go = NULL;
+ if(s_direction == "STRAIGHT" ||
+ s_direction == "FORWARD" ||
+ s_direction == "FORWARDS" ||
+ s_direction == "AROUND")
+ go = new MsgCmdGoToRelativePP(vec);
+ else if(s_direction.substr(0, 5) == "RIGHT")
+ go = new MsgCmdGoToRelativePP(vec.rotate(-PI/2));
+ else if(s_direction.substr(0, 4) == "LEFT")
+ go = new MsgCmdGoToRelativePP(vec.rotate(PI/2));
+ else if(s_direction == "BACK" ||
+ s_direction == "BACKWARD" ||
+ s_direction == "BACKWARDS")
+ go = new MsgCmdGoToRelativePP(vec.rotate(PI));
+ if(go != NULL) {
+ Boeing::RobotClient *r = p_client->find(robot);
+ if(r)
#ifdef USE_TXT_COMMANDS
- sendAction(r, go);
+ sendAction(r, go);
#else
- r->sendGoToCmd(go->content()->priority, go->content()->taskid,
- go->content()->x, go->content()->y, go->content()->angle,
- go->content()->useAngle(), go->content()->useRelative());
+ r->sendGoToCmd(go->content()->priority, go->content()->taskid,
+ go->content()->x, go->content()->y, go->content()->angle,
+ go->content()->useAngle(), go->content()->useRelative());
#endif
- delete go;
- } else
- GalUtil_Error("unknown direction %s", s_direction.c_str());
- }
- }
- }
- else if(!strcmp(query, "halt_command")) {
- GalUtil_Error("sending halt query to backend");
- vector<string> bots = p_client->getClientList();
- for(vector<string>::const_iterator i = bots.begin(); i != bots.end(); i++) {
- Boeing::RobotClient *r = p_client->find(*i);
- MsgCmdHaltPP halt;
- if (r)
+ delete go;
+ } else
+ GalUtil_Error("unknown direction %s", s_direction.c_str());
+ }
+ }
+}
+
+static void halt_command(const char* robot, const Gal_Frame& inframe)
+{
+ GalUtil_Error("sending halt query to backend");
+ vector<string> bots = p_client->getClientList();
+ for(vector<string>::const_iterator i = bots.begin(); i != bots.end(); i++) {
+ Boeing::RobotClient *r = p_client->find(*i);
+ MsgCmdHaltPP halt;
+ if (r)
#ifdef USE_TXT_COMMANDS
- sendAction(r, &halt);
+ sendAction(r, &halt);
#else
- r->sendHalt(halt.content()->priority, halt.content()->taskid);
+ r->sendHalt(halt.content()->priority, halt.content()->taskid);
#endif
- }
- }
- else if(!strcmp(query, "follow_command")) {
- GalUtil_Error("sending follow command to backend");
- string followee = Gal_GetString(inframe, ":followee");
- if(followee.empty()) {
- cerr << "no followee" << endl;
- } else {
- MsgCmdFollowPP follow(followee);
- Boeing::RobotClient *r = p_client->find(robot);
- if(r)
+ }
+}
+
+static void follow_command(const char* robot, const Gal_Frame& inframe)
+{
+ GalUtil_Error("sending follow command to backend");
+ string followee = Gal_GetString(inframe, ":followee");
+ if(followee.empty()) {
+ cerr << "no followee" << endl;
+ } else {
+ MsgCmdFollowPP follow(followee);
+ Boeing::RobotClient *r = p_client->find(robot);
+ if(r)
#ifdef USE_TXT_COMMANDS
- sendAction(r, &follow);
+ sendAction(r, &follow);
#else
- r->sendFollow(follow.content()->priority, follow.content()->taskid);
+ r->sendFollow(follow.content()->priority, follow.content()->taskid);
#endif
- }
- }
- else if(!strcmp(query, "search_command") || !strcmp(query, "explore_command")) {
- if(!strcmp(query, "search_command")) {
- GalUtil_Debug1("sending search command to backend\n");
- } else {
- GalUtil_Debug1("sending explore command to backend\n");
- }
- geometry::Polygon polygon;
- ostringstream search;
- if(!strcmp(query, "search_command")) {
- search << "search";
- } else {
- search << "explore";
- }
- for(int i=1;;i++) {
- ostringstream xparam;
- xparam << ":x" << i;
- GalUtil_Debug1("xparam is %s\n", xparam.str().c_str());
- char* sx = Gal_GetString(inframe, xparam.str().c_str());
- ostringstream yparam;
- yparam << ":y" << i;
- char* sy = Gal_GetString(inframe, yparam.str().c_str());
- if(sx == NULL || !strcmp(sx, "<UNDEFINED>") || sy == NULL || !strcmp(sy, "<UNDEFINED>")) break;
- //polygon.push_back(Point(atof(sx), atof(sy)));
- //GalUtil_Debug1("x = %f, y = %f\n", polygon.back().x, polygon.back().y);
- search << " (" << sx << ' ' << sy << ')';
- }
- //MsgCmdCoverPP mcc(polygon);
- //MsgCmdActionPP mcc = Msg::construct_MsgCmdAction(search.str());
- //p_client->sendPacketToOptrader(mcc);
- t_client->sendTask(iTraderTaskId++, search.str().c_str());
- }
- else if(!strcmp(query, "move_to_goal_command")) {
- GalUtil_Error("sending goal movement to backend");
- string ordinal(Gal_GetString(inframe, ":ordinal"));
- if(ordinal == "home") {
- string sRelDist(Gal_GetString(inframe, ":relDist"));
- if(sRelDist == "1") {
- MsgCmdHomePP home;
- Boeing::RobotClient *r = p_client->find(robot);
- if(r) r->sendHome(home.content()->priority, home.content()->taskid);
- //p_client->sendPacket(robot, MsgCmdHomePP());
- } else if(sRelDist == "2") {
- ostringstream err;
- err << "relative to goal '" << sRelDist << '\'' << " not understood";
- GalUtil_Error(err.str().c_str());
- return f;
- } else if(sRelDist == "3") {
- ostringstream err;
- err << "relative to goal '" << sRelDist << '\'' << " not understood";
- GalUtil_Error(err.str().c_str());
- return f;
- } else if(sRelDist == "4") {
- ostringstream err;
- err << "relative to goal '" << sRelDist << '\'' << " not understood";
- GalUtil_Error(err.str().c_str());
- return f;
- } else if(sRelDist == "5") {
- ostringstream err;
- err << "relative to goal '" << sRelDist << '\'' << " not understood";
- GalUtil_Error(err.str().c_str());
- return f;
- } else if(sRelDist == "6") {
- ostringstream err;
- err << "relative to goal '" << sRelDist << '\'' << " not understood";
- GalUtil_Error(err.str().c_str());
- return f;
- } else {
- ostringstream err;
- err << "relative to goal '" << sRelDist << '\'' << " not understood";
- GalUtil_Error(err.str().c_str());
- return f;
- }
- }
- else {
- //absolute ordinal
- istringstream issOrdinal(ordinal);
- string token;
- issOrdinal >> token;
- float f_xcoord, f_ycoord;
- if(token == "NEGATIVE") {
- GalUtil_Error("got negative x");
- issOrdinal >> token;
- ostringstream dispval;
- dispval << '"' << token << '"';
- GalUtil_Error(dispval.str().c_str());
- f_xcoord = static_cast<float>(-tkGetNumber999(token));
- } else {
- ostringstream dispval;
- dispval << '"' << token << '"';
- GalUtil_Error(dispval.str().c_str());
- f_xcoord = static_cast<float>(tkGetNumber999(token));
- }
- GalUtil_Error("got x");
- issOrdinal >> token;
- if(token == "NEGATIVE") {
- GalUtil_Error("got negative y");
- issOrdinal >> token;
- ostringstream dispval;
- dispval << '"' << token << '"';
- GalUtil_Error(dispval.str().c_str());
- f_ycoord = static_cast<float>(-tkGetNumber999(token));
- } else {
- ostringstream dispval;
- dispval << '"' << token << '"';
- GalUtil_Error(dispval.str().c_str());
- f_ycoord = static_cast<float>(tkGetNumber999(token));
- }
- GalUtil_Error("got y");
- MsgCmdGoToGlobalPP go(Point(f_xcoord, f_ycoord));
- Boeing::RobotClient *r = p_client->find(robot);
- if(r)
+ }
+}
+
+static void search_or_explore(const char* query, const char* robot, const Gal_Frame& inframe)
+{
+ if(!strcmp(query, "search_command")) {
+ GalUtil_Debug1("sending search command to backend\n");
+ } else {
+ GalUtil_Debug1("sending explore command to backend\n");
+ }
+ geometry::Polygon polygon;
+ ostringstream search;
+ if(!strcmp(query, "search_command")) {
+ search << "search";
+ } else {
+ search << "explore";
+ }
+ for(int i=1;;i++) {
+ ostringstream xparam;
+ xparam << ":x" << i;
+ GalUtil_Debug1("xparam is %s\n", xparam.str().c_str());
+ char* sx = Gal_GetString(inframe, xparam.str().c_str());
+ ostringstream yparam;
+ yparam << ":y" << i;
+ char* sy = Gal_GetString(inframe, yparam.str().c_str());
+ if(sx == NULL || !strcmp(sx, "<UNDEFINED>") || sy == NULL || !strcmp(sy, "<UNDEFINED>")) break;
+ //polygon.push_back(Point(atof(sx), atof(sy)));
+ //GalUtil_Debug1("x = %f, y = %f\n", polygon.back().x, polygon.back().y);
+ search << " (" << sx << ' ' << sy << ')';
+ }
+ //MsgCmdCoverPP mcc(polygon);
+ //MsgCmdActionPP mcc = Msg::construct_MsgCmdAction(search.str());
+ //p_client->sendPacketToOptrader(mcc);
+ t_client->sendTask(iTraderTaskId++, search.str().c_str());
+}
+
+static void move_to_goal_command(const char* robot, const Gal_Frame& inframe)
+{
+ GalUtil_Error("sending goal movement to backend");
+ string ordinal(Gal_GetString(inframe, ":ordinal"));
+ if(ordinal == "home") {
+ string sRelDist(Gal_GetString(inframe, ":relDist"));
+ if(sRelDist == "1") {
+ MsgCmdHomePP home;
+ Boeing::RobotClient *r = p_client->find(robot);
+ if(r) r->sendHome(home.content()->priority, home.content()->taskid);
+ //p_client->sendPacket(robot, MsgCmdHomePP());
+ } else if(sRelDist == "2") {
+ ostringstream err;
+ err << "relative to goal '" << sRelDist << '\'' << " not understood";
+ GalUtil_Error(err.str().c_str());
+ return;
+ } else if(sRelDist == "3") {
+ ostringstream err;
+ err << "relative to goal '" << sRelDist << '\'' << " not understood";
+ GalUtil_Error(err.str().c_str());
+ return;
+ } else if(sRelDist == "4") {
+ ostringstream err;
+ err << "relative to goal '" << sRelDist << '\'' << " not understood";
+ GalUtil_Error(err.str().c_str());
+ return;
+ } else if(sRelDist == "5") {
+ ostringstream err;
+ err << "relative to goal '" << sRelDist << '\'' << " not understood";
+ GalUtil_Error(err.str().c_str());
+ return;
+ } else if(sRelDist == "6") {
+ ostringstream err;
+ err << "relative to goal '" << sRelDist << '\'' << " not understood";
+ GalUtil_Error(err.str().c_str());
+ return;
+ } else {
+ ostringstream err;
+ err << "relative to goal '" << sRelDist << '\'' << " not understood";
+ GalUtil_Error(err.str().c_str());
+ return;
+ }
+ } else {
+ //absolute ordinal
+ istringstream issOrdinal(ordinal);
+ string token;
+ issOrdinal >> token;
+ float f_xcoord, f_ycoord;
+ if(token == "NEGATIVE") {
+ GalUtil_Error("got negative x");
+ issOrdinal >> token;
+ ostringstream dispval;
+ dispval << '"' << token << '"';
+ GalUtil_Error(dispval.str().c_str());
+ f_xcoord = static_cast<float>(-tkGetNumber999(token));
+ } else {
+ ostringstream dispval;
+ dispval << '"' << token << '"';
+ GalUtil_Error(dispval.str().c_str());
+ f_xcoord = static_cast<float>(tkGetNumber999(token));
+ }
+ GalUtil_Error("got x");
+ issOrdinal >> token;
+ if(token == "NEGATIVE") {
+ GalUtil_Error("got negative y");
+ issOrdinal >> token;
+ ostringstream dispval;
+ dispval << '"' << token << '"';
+ GalUtil_Error(dispval.str().c_str());
+ f_ycoord = static_cast<float>(-tkGetNumber999(token));
+ } else {
+ ostringstream dispval;
+ dispval << '"' << token << '"';
+ GalUtil_Error(dispval.str().c_str());
+ f_ycoord = static_cast<float>(tkGetNumber999(token));
+ }
+ GalUtil_Error("got y");
+ MsgCmdGoToGlobalPP go(Point(f_xcoord, f_ycoord));
+ Boeing::RobotClient *r = p_client->find(robot);
+ if(r)
#ifdef USE_TXT_COMMANDS
- sendAction(r, &go);
+ sendAction(r, &go);
#else
- r->sendGoToCmd(go.content()->priority, go.content()->taskid,
- go.content()->x, go.content()->y, go.content()->angle,
- go.content()->useAngle(), go.content()->useRelative());
+ r->sendGoToCmd(go.content()->priority, go.content()->taskid,
+ go.content()->x, go.content()->y, go.content()->angle,
+ go.content()->useAngle(), go.content()->useRelative());
#endif
- }
- }
- else if(!strcmp(query, "turn_command")) {
- GalUtil_Error("got turn command");
- string sDirection(Gal_GetString(inframe, ":direction"));
- bool bRel(false);
- double fRad;
- if(sDirection == "NORTH") fRad = PI/2;
- else if(sDirection == "NORTHEAST") fRad = PI/4;
- else if(sDirection == "NORTHWEST") fRad = 3*PI/4;
- else if(sDirection == "EAST") fRad = 0;
- else if(sDirection == "WEST") fRad = PI;
- else if(sDirection == "SOUTH") fRad = 3*PI/2;
- else if(sDirection == "SOUTHEAST") fRad = 7*PI/4;
- else if(sDirection == "SOUTHWEST") fRad = 5*PI/4;
- else if(sDirection == "STRAIGHT" ||
- sDirection == "FORWARD" ||
- sDirection == "FORWARDS") {
- bRel = true;
- fRad = 0;
- } else if(sDirection.substr(0, 5) == "RIGHT") {
- string sQual;
- if(Gal_GetString(inframe, ":increment"))
- sQual = (Gal_GetString(inframe, ":increment"));
- bRel = true;
- if(sQual.empty() || sQual == "<UNDEFINED>") {
- fRad = -PI/2;
- } else {
- fRad = -tkGetNumber999(sQual)*PI/180;
- }
- } else if(sDirection.substr(0, 4) == "LEFT") {
- string sQual;
- if(Gal_GetString(inframe, ":increment"))
- sQual = (Gal_GetString(inframe, ":increment"));
- bRel = true;
- if(sQual.empty() || sQual == "<UNDEFINED>") {
- fRad = PI/2;
- } else {
- fRad = tkGetNumber999(sQual)*PI/180;
- }
- } else if(sDirection == "AROUND" ||
- sDirection == "BACK" ||
- sDirection == "BACKWARD" ||
- sDirection == "BACKWARDS") {
- bRel = true;
- fRad = PI;
- } else {
- GalUtil_Error("unknown direction %s", sDirection.c_str());
- return f;
- }
- MsgCmdTurnRelativePP turn(fRad);
- Boeing::RobotClient *r = p_client->find(robot);
- if(r)
+ }
+}
+
+
+
+static void turn_command(const char* robot, const Gal_Frame& inframe)
+{
+ GalUtil_Error("got turn command");
+ string sDirection(Gal_GetString(inframe, ":direction"));
+ bool bRel(false);
+ double fRad;
+ if(sDirection == "NORTH") fRad = PI/2;
+ else if(sDirection == "NORTHEAST") fRad = PI/4;
+ else if(sDirection == "NORTHWEST") fRad = 3*PI/4;
+ else if(sDirection == "EAST") fRad = 0;
+ else if(sDirection == "WEST") fRad = PI;
+ else if(sDirection == "SOUTH") fRad = 3*PI/2;
+ else if(sDirection == "SOUTHEAST") fRad = 7*PI/4;
+ else if(sDirection == "SOUTHWEST") fRad = 5*PI/4;
+ else if(sDirection == "STRAIGHT" ||
+ sDirection == "FORWARD" ||
+ sDirection == "FORWARDS") {
+ bRel = true;
+ fRad = 0;
+ } else if(sDirection.substr(0, 5) == "RIGHT") {
+ string sQual;
+ if(Gal_GetString(inframe, ":increment"))
+ sQual = (Gal_GetString(inframe, ":increment"));
+ bRel = true;
+ if(sQual.empty() || sQual == "<UNDEFINED>") {
+ fRad = -PI/2;
+ } else {
+ fRad = -tkGetNumber999(sQual)*PI/180;
+ }
+ } else if(sDirection.substr(0, 4) == "LEFT") {
+ string sQual;
+ if(Gal_GetString(inframe, ":increment"))
+ sQual = (Gal_GetString(inframe, ":increment"));
+ bRel = true;
+ if(sQual.empty() || sQual == "<UNDEFINED>") {
+ fRad = PI/2;
+ } else {
+ fRad = tkGetNumber999(sQual)*PI/180;
+ }
+ } else if(sDirection == "AROUND" ||
+ sDirection == "BACK" ||
+ sDirection == "BACKWARD" ||
+ sDirection == "BACKWARDS") {
+ bRel = true;
+ fRad = PI;
+ } else {
+ GalUtil_Error("unknown direction %s", sDirection.c_str());
+ return;
+ }
+ MsgCmdTurnRelativePP turn(fRad);
+ Boeing::RobotClient *r = p_client->find(robot);
+ if(r)
#ifdef USE_TXT_COMMANDS
- sendAction(r, &turn);
+ sendAction(r, &turn);
#else
- r->sendGoToCmd(turn.content()->priority, turn.content()->taskid,
- turn.content()->x, turn.content()->y, turn.content()->angle,
- turn.content()->useAngle(), turn.content()->useRelative());
+ r->sendGoToCmd(turn.content()->priority, turn.content()->taskid,
+ turn.content()->x, turn.content()->y, turn.content()->angle,
+ turn.content()->useAngle(), turn.content()->useRelative());
#endif
- }
- else {
- GalUtil_Error("unhandled query: %s", query);
- }
- } else {
- GalUtil_Error("there is no robot");
- }
- } else {
- GalUtil_Error("there is no query");
- }
- } else {
- GalUtil_Error("couldn't find :inframe");
- }
- } else {
@@ Diff output truncated at 60000 characters. @@
More information about the TeamTalk-developers
mailing list