[TeamTalk 256]: [792] trunk/TeamTalk: Put code in to handle both row and column ordered maps
tk@edam.speech.cs.cmu.edu
tk at edam.speech.cs.cmu.edu
Thu Sep 27 02:27:55 EDT 2007
An HTML attachment was scrubbed...
URL: http://mailman.srv.cs.cmu.edu/pipermail/teamtalk-developers/attachments/20070927/337b2209/attachment-0001.html
-------------- next part --------------
Modified: trunk/TeamTalk/Agents/PenDecoder/src/edu/cmu/ravenclaw/pendecoder/MapBufferedImage.java
===================================================================
--- trunk/TeamTalk/Agents/PenDecoder/src/edu/cmu/ravenclaw/pendecoder/MapBufferedImage.java 2007-09-25 16:13:35 UTC (rev 791)
+++ trunk/TeamTalk/Agents/PenDecoder/src/edu/cmu/ravenclaw/pendecoder/MapBufferedImage.java 2007-09-27 06:27:53 UTC (rev 792)
@@ -19,6 +19,11 @@
static public final int FULL = 0;
static public final int DIFF = 1;
+ static public final short MAP_UNCHANGED = 0x00;
+ static public final short MAP_CLEAR = 0x20;
+ static public final short MAP_UNKNOWN = 0x40;
+ static public final short MAP_OCCUPIED = 0xff;
+
/**
* Creates a new instance of MapBufferedImage
*/
@@ -35,9 +40,9 @@
static public MapBufferedImage createMapBufferedImage(int w, int h) {
int colors[] = new int[256];
- for(int i=0; i<256; i++) colors[i] = Color.BLACK.getRGB();
- colors[64] = Color.GRAY.getRGB();
- colors[255] = Color.GREEN.getRGB();
+ colors[MAP_CLEAR] = Color.BLACK.getRGB();
+ colors[MAP_UNKNOWN] = Color.GRAY.getRGB();
+ colors[MAP_OCCUPIED] = Color.GREEN.getRGB();
IndexColorModel iColor =
new IndexColorModel(8, 256, colors, 0, false, 128, DataBuffer.TYPE_BYTE);
return new MapBufferedImage(w, h, TYPE_BYTE_INDEXED, iColor);
@@ -47,23 +52,16 @@
WritableRaster wr = getRaster();
int h = getHeight();
int w = getWidth();
- for(int y=0; y<h; y++) {
- for(int x=0; x<w; x++) {
- byte val = ras[y*w+x];
- wr.setSample(x, y, 0, val);
- }
- }
+ for(int i=0; i<h*w; i++) wr.setSample(i%w, i/w, 0, ras[i]);
}
public void setDiffMap(byte[] ras) {
WritableRaster wr = getRaster();
int h = getHeight();
int w = getWidth();
- for(int y=0; y<h; y++) {
- for(int x=0; x<w; x++) {
- byte val = ras[y*w+x];
- if(val!=0) wr.setSample(x, y, 0, val);
- }
+ for(int i=0; i<h*w; i++) {
+ byte val = ras[i];
+ if(val!=0) wr.setSample(i%w, i/w, 0, val);
}
}
}
\ No newline at end of file
Modified: trunk/TeamTalk/Agents/PenDecoder/src/edu/cmu/ravenclaw/pendecoder/MapCanvas.java
===================================================================
--- trunk/TeamTalk/Agents/PenDecoder/src/edu/cmu/ravenclaw/pendecoder/MapCanvas.java 2007-09-25 16:13:35 UTC (rev 791)
+++ trunk/TeamTalk/Agents/PenDecoder/src/edu/cmu/ravenclaw/pendecoder/MapCanvas.java 2007-09-27 06:27:53 UTC (rev 792)
@@ -25,6 +25,8 @@
*/
public class MapCanvas extends JComponent implements Scrollable {
+ public enum RowOrder {Y_MAJOR, X_MAJOR};
+
/**
* Constructor for map canvas.
* @param penDecoderDisplay Link to parent class.
@@ -274,6 +276,7 @@
/**
* Update the map.
* @param type Is this a full map or just a diff?
+ * @param row_order whether the map is encoded as y-major or x-major
* @param x_dim The width of the map (in cells)
* @param y_dim The height of the map (in cells)
* @param x_origin The vector from the left-hand side of the map to the y-axis
@@ -281,9 +284,9 @@
* @param resolution The length of a side of a cell (in centimeters)
* @param encoded_map The run-length compressed map
*/
- public void mapUpdate(int type, int x_dim, int y_dim, float x_origin, float y_origin, int resolution, int[] encoded_map) {
+ public void mapUpdate(int type, RowOrder row_order, int x_dim, int y_dim, float x_origin, float y_origin, int resolution, int[] encoded_map) {
byte[] decoded_map = new byte[x_dim * y_dim];
- decodeMap(decoded_map, x_dim, y_dim, encoded_map);
+ if (!decodeMap(decoded_map, row_order, x_dim, y_dim, encoded_map)) return;
if (type == MapBufferedImage.FULL) {
if (image_width_in_pixels != x_dim || image_height_in_pixels != y_dim) {
int colors[] = new int[3];
@@ -330,20 +333,29 @@
* @param x The width of the map (in cells)
* @param y The height of the map (in cells)
* @param encoded_map The run-length compressed map
+ * @return true if the map has any changes
*/
- protected void decodeMap(byte[] decoded_map, int x, int y, int[] encoded_map) {
- System.err.println("x: " + x + " y: " + y + " encoded_len: " + encoded_map.length);
- int j = 0, k = 0;
+ protected boolean decodeMap(byte[] decoded_map, RowOrder row_order, int x, int y, int[] encoded_map) {
+ int j = 0;
+ boolean dirty = false;
for (int i = 0; i < encoded_map.length; i++) {
- int run_length = (encoded_map[i] & -256) >> 8;
- byte run_value = (byte) (encoded_map[i] & 255);
- if (i == 0)
- System.err.println("raw: " + encoded_map[i] + " rl: " + run_length + " rv: " + run_value + " j: " + j);
- for (; j < k + run_length; j++) {
- decoded_map[j] = run_value;
+ int run_length = (encoded_map[i] & 0xFFFFFF00) >> 8;
+ int run_terminus = j+run_length;
+ byte run_value = (byte) (encoded_map[i] & 0x000000FF);
+ dirty |= run_value != MapBufferedImage.MAP_UNCHANGED;
+ switch(row_order) {
+ case Y_MAJOR:
+ Arrays.fill(decoded_map, j, run_terminus, run_value);
+ j = run_terminus;
+ break;
+ case X_MAJOR:
+ for (; j < run_terminus; j++) decoded_map[(j%y)*x+j/y] = run_value;
+ break;
+ default:
+ System.err.println("unknown row_order: " + row_order);
}
- k = j;
}
+ return dirty;
}
/**
@@ -461,7 +473,7 @@
public void setPreferredSize(Dimension d) {
view_size = d;
map_to_screen.setToScale(view_size.width / native_width, view_size.height / native_height);
- map_to_screen.translate(-x_origin, -y_origin);
+ map_to_screen.translate(-x_origin, -y_origin);
try {
screen_to_map = map_to_screen.createInverse();
} catch (NoninvertibleTransformException e) {
Modified: trunk/TeamTalk/Agents/PenDecoder/src/edu/cmu/ravenclaw/pendecoder/PenDecoderDisplay.java
===================================================================
--- trunk/TeamTalk/Agents/PenDecoder/src/edu/cmu/ravenclaw/pendecoder/PenDecoderDisplay.java 2007-09-25 16:13:35 UTC (rev 791)
+++ trunk/TeamTalk/Agents/PenDecoder/src/edu/cmu/ravenclaw/pendecoder/PenDecoderDisplay.java 2007-09-27 06:27:53 UTC (rev 792)
@@ -292,6 +292,7 @@
/**
* Update map image with new map.
* @param type full image or image diff
+ * @param row_order whether the map is encoded as y-major or x-major
* @param x_dim width of map (in meters)
* @param y_dim height of map (in meters)
* @param x_origin distance (in meters) from the left-hand side of the map to
@@ -301,8 +302,8 @@
* also the size of a side of the map cells
* @param encoded_map the map, which may be compressed or otherwize encoded
*/
- public void mapUpdate(int type, int x_dim, int y_dim, float x_origin, float y_origin, int resolution, int[] encoded_map) {
- mapCanvas.mapUpdate(type, x_dim, y_dim, x_origin, y_origin, resolution, encoded_map);
+ public void mapUpdate(int type, MapCanvas.RowOrder row_order, int x_dim, int y_dim, float x_origin, float y_origin, int resolution, int[] encoded_map) {
+ mapCanvas.mapUpdate(type, row_order, x_dim, y_dim, x_origin, y_origin, resolution, encoded_map);
}
/**
Modified: trunk/TeamTalk/Agents/PenDecoder/src/edu/cmu/ravenclaw/pendecoder/PenDecoderServer.java
===================================================================
--- trunk/TeamTalk/Agents/PenDecoder/src/edu/cmu/ravenclaw/pendecoder/PenDecoderServer.java 2007-09-25 16:13:35 UTC (rev 791)
+++ trunk/TeamTalk/Agents/PenDecoder/src/edu/cmu/ravenclaw/pendecoder/PenDecoderServer.java 2007-09-27 06:27:53 UTC (rev 792)
@@ -200,6 +200,7 @@
public GFrame serverOpMapUpdate(GFrame f) {
System.err.println("got map update");
String type = (String)f.getProperty(":type");
+ MapCanvas.RowOrder row_order = MapCanvas.RowOrder.valueOf((String)f.getProperty(":order"));
int x_dim = ((Integer)f.getProperty(":x_size")).intValue();
int y_dim = ((Integer)f.getProperty(":y_size")).intValue();
float x_origin = ((Float)f.getProperty(":x_origin")).floatValue();
@@ -208,10 +209,10 @@
int compressed_map[] = f.getInt32(":encoded_map").getIntArray();
if(type.equals(new String("full"))) {
System.err.println("got a full map");
- frame.mapUpdate(MapBufferedImage.FULL, x_dim, y_dim, x_origin, y_origin, resolution, compressed_map);
+ frame.mapUpdate(MapBufferedImage.FULL, row_order, x_dim, y_dim, x_origin, y_origin, resolution, compressed_map);
} else if(type.equals(new String("diff"))) {
System.err.println("got a diff map");
- frame.mapUpdate(MapBufferedImage.DIFF, x_dim, y_dim, x_origin, y_origin, resolution, compressed_map);
+ frame.mapUpdate(MapBufferedImage.DIFF, row_order, x_dim, y_dim, x_origin, y_origin, resolution, compressed_map);
} else {
System.err.println("got unknow update type: " + type);
}
Modified: trunk/TeamTalk/Agents/TeamTalkBackend/backendstub/backendstub.cpp
===================================================================
--- trunk/TeamTalk/Agents/TeamTalkBackend/backendstub/backendstub.cpp 2007-09-25 16:13:35 UTC (rev 791)
+++ trunk/TeamTalk/Agents/TeamTalkBackend/backendstub/backendstub.cpp 2007-09-27 06:27:53 UTC (rev 792)
@@ -68,7 +68,7 @@
}
switch(msg->hdr.type) {
case Boeing::MAP_SUBSCRIBE:
- Boeing::MsgMap::MsgMapFactory(Boeing::MAP_RLE,
+ Boeing::MsgMap::MsgMapFactory(Boeing::MAP_Y_MAJOR,
msgMap, mapData, mapWidth*mapHeight,
0, 0, mapWidth, mapHeight);
debug << "raw: " << msgMap->map[0]
Modified: trunk/TeamTalk/Agents/TeamTalkBackend/robot-galaxy_adapter.cpp
===================================================================
--- trunk/TeamTalk/Agents/TeamTalkBackend/robot-galaxy_adapter.cpp 2007-09-25 16:13:35 UTC (rev 791)
+++ trunk/TeamTalk/Agents/TeamTalkBackend/robot-galaxy_adapter.cpp 2007-09-27 06:27:53 UTC (rev 792)
@@ -7,7 +7,7 @@
//adds undiscoverable robots from a file
vector<string> GalaxyRobots::processPeerfile(const string& fname)
{
- vector<string> names;
+ vector<string> names;
debug << "about to add robots" << endl;
ifstream Frobotips(fname.c_str());
if(!Frobotips) error << "problem opening " << fname << endl;
@@ -32,49 +32,49 @@
info << "adding optrader@" << rip << ':' << port << endl;
t_client->open(rip.c_str(), port);
} else if(rname == "MAPSERVER") {
- ignoreToEndOfLine(Frobotips);
- info << "adding mapserver@" << rip << ':' << port << endl;
- if(port) m_client->open(rip.c_str(), port);
- else m_client->open(rip.c_str());
- } else {
+ ignoreToEndOfLine(Frobotips);
+ info << "adding mapserver@" << rip << ':' << port << endl;
+ if(port) m_client->open(rip.c_str(), port);
+ else m_client->open(rip.c_str());
+ } else {
string voice, safeness;
Frobotips >> voice >> safeness;
ignoreToEndOfLine(Frobotips);
info << "adding robot@" << rip << ':' << port << ' ' << voice << ' ' << safeness << endl;
- if(!r_client->addRobot(rname, rip, voice, safeness, port, this)) {
- error << "couldn't add " << rname << endl;
- } else {
- names.push_back(rname);
- }
+ if(!r_client->addRobot(rname, rip, voice, safeness, port, this)) {
+ error << "couldn't add " << rname << endl;
+ } else {
+ names.push_back(rname);
+ }
}
- } catch(TeamTalk::RobotClientException e) {
+ } catch(TeamTalk::RobotClientException e) {
error << "unable to add robot " << rname << '(' << rip << ')' << endl;
error << e.what() << endl;
}
}
- return names;
+ return names;
}
void GalaxyRobots::addRobotNamesToGrammar(const vector<string>& names)
{
- debug << "adding robot names to grammar:";
- ofstream namefile("..\\..\\Resources\\Grammar\\TeamTalkRobots");
- for(vector<string>::const_iterator i = names.begin(); i != names.end(); i++) {
- namefile << tolower(*i) << endl;
+ debug << "adding robot names to grammar:";
+ ofstream namefile("..\\..\\Resources\\Grammar\\TeamTalkRobots");
+ for(vector<string>::const_iterator i = names.begin(); i != names.end(); i++) {
+ namefile << tolower(*i) << endl;
}
PROCESS_INFORMATION lm_build_proc =
- spawn(true, "Language Build", "..\\..\\Tools\\MakeLM", "perl", "makelm.pl");
+ spawn(true, "Language Build", "..\\..\\Tools\\MakeLM", "perl", "makelm.pl");
}
void GalaxyRobots::traderlistener(void *p)
{
- GalaxyRobots* me = (GalaxyRobots *)p;
- Boeing::TraderClient* t_client = me->t_client;
-
- //this is bogus, but need to ping trader to get on its client list
- t_client->sendCancel(0);
+ GalaxyRobots* me = (GalaxyRobots *)p;
+ Boeing::TraderClient* t_client = me->t_client;
+ //this is bogus, but need to ping trader to get on its client list
+ t_client->sendCancel(0);
+
for(;;) {
const Boeing::MsgTraderClient* msg = t_client->getNextMessage();
if(!msg) {
@@ -82,70 +82,70 @@
continue;
}
Gal_Frame f = Gal_MakeFrame("trader_message", GAL_CLAUSE);
- if(msg->hdr.type == Boeing::INFO) {
- Gal_SetProp(f, ":type", Gal_StringObject("info"));
- debug << "got info message: " << msg->info.task << endl;
- istringstream input(msg->info.task);
- string token;
- string ip;
- string object;
- float x, y;
- input >> token;
- debug << "got info token " << token << endl;
- if(token != "object") {
- ip = token;
- input >> token;
- if(token != "object") {
- error << "was expecting 'object'" << endl;
- continue;
- }
- }
- input >> object;
- debug << "got info object " << object << endl;
- input >> token;
- debug << "got info token " << token << endl;
- if(token != "at") {
- error << "was expecting 'at'" << endl;
- continue;
- }
- input >> token;
- debug << "got info token " << token << endl;
- if(token.empty() || token.at(0) != '(') {
- error << "was expecting (..." << endl;
- continue;
- }
- if(token == "(") {
- input >> token;
- debug << "got info token " << token << endl;
- x = (float)atof(token.c_str());
- } else {
- x = (float)atof(token.c_str()+1);
- }
- debug << "got info x: " << x << endl;
- input >> token;
- debug << "got info token " << token << endl;
- if(token.empty()) {
- error << "was expecting ...)" << endl;
- continue;
- } else if(token.at(token.length()-1) != ')') {
- token.resize(token.length()-1);
- }
- y = (float)atof(token.c_str());
- 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));
- } else if(msg->hdr.type == Boeing::TASK_ACK) {
- Gal_SetProp(f, ":type", Gal_StringObject("ack"));
- Gal_SetProp(f, ":taskid", Gal_IntObject(msg->ack.taskid));
- debug << "got task ack: " << msg->ack.taskid << endl;
- } else if(msg->hdr.type == Boeing::TASK_DONE) {
- Gal_SetProp(f, ":type", Gal_StringObject("done"));
- Gal_SetProp(f, ":taskid", Gal_IntObject(msg->done.taskid));
- Gal_SetProp(f, ":status", Gal_IntObject(msg->done.status));
- debug << "got task done: " << msg->done.taskid << ' ' << msg->done.status << endl;
- } else {
- warn << "unknown message type '" << msg->hdr.type << "' in traderlistener" << endl;
+ if(msg->hdr.type == Boeing::INFO) {
+ Gal_SetProp(f, ":type", Gal_StringObject("info"));
+ debug << "got info message: " << msg->info.task << endl;
+ istringstream input(msg->info.task);
+ string token;
+ string ip;
+ string object;
+ float x, y;
+ input >> token;
+ debug << "got info token " << token << endl;
+ if(token != "object") {
+ ip = token;
+ input >> token;
+ if(token != "object") {
+ error << "was expecting 'object'" << endl;
+ continue;
+ }
+ }
+ input >> object;
+ debug << "got info object " << object << endl;
+ input >> token;
+ debug << "got info token " << token << endl;
+ if(token != "at") {
+ error << "was expecting 'at'" << endl;
+ continue;
+ }
+ input >> token;
+ debug << "got info token " << token << endl;
+ if(token.empty() || token.at(0) != '(') {
+ error << "was expecting (..." << endl;
+ continue;
+ }
+ if(token == "(") {
+ input >> token;
+ debug << "got info token " << token << endl;
+ x = (float)atof(token.c_str());
+ } else {
+ x = (float)atof(token.c_str()+1);
+ }
+ debug << "got info x: " << x << endl;
+ input >> token;
+ debug << "got info token " << token << endl;
+ if(token.empty()) {
+ error << "was expecting ...)" << endl;
+ continue;
+ } else if(token.at(token.length()-1) != ')') {
+ token.resize(token.length()-1);
+ }
+ y = (float)atof(token.c_str());
+ 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));
+ } else if(msg->hdr.type == Boeing::TASK_ACK) {
+ Gal_SetProp(f, ":type", Gal_StringObject("ack"));
+ Gal_SetProp(f, ":taskid", Gal_IntObject(msg->ack.taskid));
+ debug << "got task ack: " << msg->ack.taskid << endl;
+ } else if(msg->hdr.type == Boeing::TASK_DONE) {
+ Gal_SetProp(f, ":type", Gal_StringObject("done"));
+ Gal_SetProp(f, ":taskid", Gal_IntObject(msg->done.taskid));
+ Gal_SetProp(f, ":status", Gal_IntObject(msg->done.status));
+ debug << "got task done: " << msg->done.taskid << ' ' << msg->done.status << endl;
+ } else {
+ warn << "unknown message type '" << msg->hdr.type << "' in traderlistener" << endl;
Gal_FreeFrame(f);
continue;
}
@@ -156,8 +156,8 @@
void GalaxyRobots::maplistener(void *p)
{
- GalaxyRobots* me = (GalaxyRobots*)p;
- Boeing::MapClient* m_client = me->m_client;
+ GalaxyRobots* me = (GalaxyRobots*)p;
+ Boeing::MapClient* m_client = me->m_client;
for(int t=0;;) {
const Boeing::MsgMapClient* msg = m_client->getNextMessage();
if(!msg) {
@@ -173,20 +173,32 @@
Gal_SetProp(f, ":type", Gal_StringObject("diff"));
break;
default:
- error << "unknown message type '" << msg->hdr.type << "' in maplistener" << endl;
+ error << "unknown message type '" << msg->hdr.type << "' in maplistener" << endl;
Gal_FreeFrame(f);
continue;
}
+ switch(msg->map.encoding) {
+ case Boeing::MAP_Y_MAJOR:
+ Gal_SetProp(f, ":order", Gal_StringObject("Y_MAJOR"));
+ break;
+ case Boeing::MAP_X_MAJOR:
+ Gal_SetProp(f, ":order", Gal_StringObject("X_MAJOR"));
+ break;
+ default:
+ error << "unhandled order: " << msg->map.encoding << endl;
+ 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, ":x_origin", Gal_FloatObject(msg->map.x_origin));
+ Gal_SetProp(f, ":x_origin", Gal_FloatObject(msg->map.x_origin));
Gal_SetProp(f, ":y_origin", Gal_FloatObject(msg->map.y_origin));
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];
- debug << "got raw: " << temp[0]
- << " rl: " << ((temp[0]&0xFFFFFF00)>>8)
- << " rv: " << (temp[0]&0x000000FF) << endl;
+ debug << "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));
me->writeFrameSkeletonHub(f);
@@ -196,8 +208,8 @@
void GalaxyRobots::trackmap(void *thisp)
{
- GalaxyRobots* me = (GalaxyRobots*)thisp;
- Boeing::MapClient* m_client = me->m_client;
+ GalaxyRobots* me = (GalaxyRobots*)thisp;
+ Boeing::MapClient* m_client = me->m_client;
debug << "starting trackmap thread" << endl;
for(int t=0; true; t++) {
Sleep(1000);
@@ -209,11 +221,11 @@
void GalaxyRobots::writeFrameAllHubs(Gal_Frame f)
{
- debug << "writing to all hubs" << endl;
+ debug << "writing to all hubs" << endl;
for(set<Agent>::iterator i = agents_.begin(); i != agents_.end(); i++) {
- i->writeFrame(f);
- }
- writeFrameSkeletonHub(f);
+ i->writeFrame(f);
+ }
+ writeFrameSkeletonHub(f);
};
void GalaxyRobots::writeFrameSkeletonHub(Gal_Frame f)
@@ -223,73 +235,73 @@
GalIO_CommWriteFrame(skeleton_comm_, f, GAL_FALSE);
return;
}
- error << "skeleton unavailable" << endl;
+ error << "skeleton unavailable" << endl;
Sleep(1000);
}
};
GalaxyRobots::GalaxyRobots(Gal_Server *s, int argc, char **argv)
{
- skeleton_comm_ = NULL;
+ skeleton_comm_ = NULL;
- iTraderTaskId = rand();
+ iTraderTaskId = rand();
- debug << "about to get the p_client" << endl;
- try {
- t_client = new Boeing::TraderClient();
- m_client = new Boeing::MapClient();
+ debug << "about to get the p_client" << endl;
+ try {
+ t_client = new Boeing::TraderClient();
+ m_client = new Boeing::MapClient();
- //startup robot client
- TeamTalk::RobotClient::spawnback = &Agent::spawnMinorGalaxy;
- r_client = new TeamTalk::RobotsClient("tester");
- vector<string> names = processPeerfile("peerfile.txt");
- if(!testLastConfig("peerfile.txt",
- "..\\..\\Resources\\Grammar\\TeamTalkRobots")) {
- addRobotNamesToGrammar(names);
- }
- if(!m_client->isConnected()) {
- fatal << "mapserver not found" << endl;
- } else if(!m_client->sendSubscribe(0)) {
- fatal << "couldn't send subscribe message to map server" << endl;
- } else {
- _beginthread(maplistener, 0, (void*) this);
- _beginthread(trackmap, 0, (void*) this);
- }
- if(!t_client->isConnected()) {
- fatal << "optraderserver not found" << endl;
- } else {
- debug << "connected to trader" << endl;
- //start listening threads
- _beginthread(traderlistener, 0, (void*) this);
- }
- } catch(exception e) {
- fatal << "couldn't start GalaxyRobots: " << e.what() << ": " << endl;
- throw e;
- }
+ //startup robot client
+ TeamTalk::RobotClient::spawnback = &Agent::spawnMinorGalaxy;
+ r_client = new TeamTalk::RobotsClient("tester");
+ vector<string> names = processPeerfile("peerfile.txt");
+ if(!testLastConfig("peerfile.txt",
+ "..\\..\\Resources\\Grammar\\TeamTalkRobots")) {
+ addRobotNamesToGrammar(names);
+ }
+ if(!m_client->isConnected()) {
+ fatal << "mapserver not found" << endl;
+ } else if(!m_client->sendSubscribe(0)) {
+ fatal << "couldn't send subscribe message to map server" << endl;
+ } else {
+ _beginthread(maplistener, 0, (void*) this);
+ _beginthread(trackmap, 0, (void*) this);
+ }
+ if(!t_client->isConnected()) {
+ fatal << "optraderserver not found" << endl;
+ } else {
+ debug << "connected to trader" << endl;
+ //start listening threads
+ _beginthread(traderlistener, 0, (void*) this);
+ }
+ } catch(exception e) {
+ fatal << "couldn't start GalaxyRobots: " << e.what() << ": " << endl;
+ throw e;
+ }
}
GalaxyRobots::~GalaxyRobots() {
-//kill all of the spawned children
- while(!Agent::serverChildren_.empty()) {
- TerminateProcess(Agent::serverChildren_.top(),1);
- Agent::serverChildren_.pop();
+ //kill all of the spawned children
+ while(!Agent::serverChildren_.empty()) {
+ TerminateProcess(Agent::serverChildren_.top(),1);
+ Agent::serverChildren_.pop();
}
}
void GalaxyRobots::setSkeletonComm(GalIO_CommStruct* comm)
{
- skeleton_comm_ = comm;
+ skeleton_comm_ = comm;
}
void GalaxyRobots::setRobotVoices()
{
- debug << "attempting to set robot voices" << endl;
- for(TeamTalk::RobotsClient::const_iterator i = r_client->begin();
- i != r_client->end(); i++) {
- string name(toupper((*i)->getName()));
- string voice(toupper((*i)->getVoice()));
- debug << "adding voice " << voice << " to name " << name << endl;
- SendRobotConfigMessage(name, voice);
+ debug << "attempting to set robot voices" << endl;
+ for(TeamTalk::RobotsClient::const_iterator i = r_client->begin();
+ i != r_client->end(); i++) {
+ string name(toupper((*i)->getName()));
+ string voice(toupper((*i)->getVoice()));
+ debug << "adding voice " << voice << " to name " << name << endl;
+ SendRobotConfigMessage(name, voice);
}
}
@@ -297,7 +309,7 @@
// [2006-07-22] (tk): send a robot_config message to galaxy hubs
//-------------------------------------------------------------------
void GalaxyRobots::SendRobotConfigMessage(const string& sName,
- const string& sVoice)
+ const string& sVoice)
{
// variables holding the nested galaxy parse frame
Gal_Frame f = Gal_MakeFrame("robot_config", GAL_CLAUSE);
@@ -309,24 +321,24 @@
void GalaxyRobots::addRobot(const string& name, GalIO_CommStruct* comm)
{
- TeamTalk::RobotClient *robotClient = r_client->find(name);
- if(!robotClient) error << "Couldn't find robotclient for: " << name << endl;
- else agents_.insert(Agent(robotClient, comm));
+ TeamTalk::RobotClient *robotClient = r_client->find(name);
+ if(!robotClient) error << "Couldn't find robotclient for: " << name << endl;
+ else agents_.insert(Agent(robotClient, comm));
}
void GalaxyRobots::processRobotMessage(const Msg* m)
{
- Agent* agent = find(m->getSender());
- if(agent == NULL) {
- error << "agent not found for: " << m->getSender() << endl;
- error << "agents are: ";
- for(set<Agent>::const_iterator i = agents_.begin(); i != agents_.end(); i++)
- error << '"' << i->getName() << "\" ";
- error << endl;
- return;
- }
- agent->processMessage(m);
- Gal_Frame f = Agent::galaxyFrame(m);
+ Agent* agent = find(m->getSender());
+ if(agent == NULL) {
+ error << "agent not found for: " << m->getSender() << endl;
+ error << "agents are: ";
+ for(set<Agent>::const_iterator i = agents_.begin(); i != agents_.end(); i++)
+ error << '"' << i->getName() << "\" ";
+ error << endl;
+ return;
+ }
+ agent->processMessage(m);
+ Gal_Frame f = Agent::galaxyFrame(m);
char *frame_name = Gal_FrameName(f);
if(frame_name != NULL && strcmp(frame_name, "robot_message")) {
debug << "sending to skeleton hub: " << m->render() << endl;
@@ -338,9 +350,9 @@
void GalaxyRobots::halt_command()
{
debug << "sending halt query to backend" << endl;
- for(set<Agent>::iterator i = agents_.begin(); i != agents_.end(); i++) {
+ for(set<Agent>::iterator i = agents_.begin(); i != agents_.end(); i++) {
i->halt_command();
- }
+ }
}
void GalaxyRobots::search_or_explore(const char* query, const Gal_Frame& inframe)
@@ -377,24 +389,24 @@
void GalaxyRobots::cancel_task(const Gal_Frame& inframe)
{
- debug << "sending cancel task to backend" << endl;
- const char* s = Gal_GetString(inframe, ":taskid");
- if(!s) {
- error << "no taskid for cancel command" << endl;
- return;
- }
- istringstream in(s);
- int taskid;
- if((in >> taskid).fail()) {
- error << "couldn't parse taskid: " << s << endl;
- return;
- }
- t_client->sendCancel(taskid);
+ debug << "sending cancel task to backend" << endl;
+ const char* s = Gal_GetString(inframe, ":taskid");
+ if(!s) {
+ error << "no taskid for cancel command" << endl;
+ return;
+ }
+ istringstream in(s);
+ int taskid;
+ if((in >> taskid).fail()) {
+ error << "couldn't parse taskid: " << s << endl;
+ return;
+ }
+ t_client->sendCancel(taskid);
}
void GalaxyRobots::processGalaxyMessage(const Gal_Frame& inframe)
{
- char* query = Gal_GetString(inframe, ":query");
+ char* query = Gal_GetString(inframe, ":query");
char* robot = Gal_GetString(inframe, ":robot_name");
if(!query) {
error << "there is no query" << endl;
@@ -408,40 +420,40 @@
}
debug << "robot is " << robot << endl;
- Agent* agent = find(robot);
- if(!agent) {
- error << "cannot find client for robot: " << robot << endl;
- return;
- }
+ Agent* agent = find(robot);
+ if(!agent) {
+ error << "cannot find client for robot: " << robot << endl;
+ return;
+ }
if(!strcmp(query, "location_query"))
- agent->location_query();
+ agent->location_query();
else if(!strcmp(query, "move_cardinal_command"))
- agent->move_cardinal_command(inframe);
+ agent->move_cardinal_command(inframe);
else if(!strcmp(query, "move_relative_command"))
- agent->move_relative_command(inframe);
+ agent->move_relative_command(inframe);
else if(!strcmp(query, "halt_command"))
- halt_command();
+ halt_command();
else if(!strcmp(query, "follow_command"))
- agent->follow_command(inframe);
+ agent->follow_command(inframe);
else if(!strcmp(query, "search_command") || !strcmp(query, "explore_command"))
search_or_explore(query, inframe);
else if(!strcmp(query, "move_to_goal_command"))
- agent->move_to_goal_command(inframe);
+ agent->move_to_goal_command(inframe);
else if(!strcmp(query, "turn_command"))
- agent->turn_command(inframe);
+ agent->turn_command(inframe);
else if(!strcmp(query, "setpos"))
- agent->set_pose_command(inframe);
- else if(!strcmp(query, "cancel_task"))
- cancel_task(inframe);
+ agent->set_pose_command(inframe);
+ else if(!strcmp(query, "cancel_task"))
+ cancel_task(inframe);
else error << "unhandled query: " << query << endl;
}
Agent* GalaxyRobots::find(const string& name) {
- set<Agent>::iterator i = agents_.find(Agent(name));
- return i == agents_.end()? NULL: &*i;
+ set<Agent>::iterator i = agents_.find(Agent(name));
+ return i == agents_.end()? NULL: &*i;
}
const Agent* GalaxyRobots::find(const string& name) const {
- set<Agent>::const_iterator i = agents_.find(Agent(name));
- return i == agents_.end()? NULL: &*i;
+ set<Agent>::const_iterator i = agents_.find(Agent(name));
+ return i == agents_.end()? NULL: &*i;
}
Modified: trunk/TeamTalk/Configurations/DesktopConfiguration/TeamTalk-hub-desktop-skeleton.pgm
===================================================================
--- trunk/TeamTalk/Configurations/DesktopConfiguration/TeamTalk-hub-desktop-skeleton.pgm 2007-09-25 16:13:35 UTC (rev 791)
+++ trunk/TeamTalk/Configurations/DesktopConfiguration/TeamTalk-hub-desktop-skeleton.pgm 2007-09-27 06:27:53 UTC (rev 792)
@@ -181,7 +181,7 @@
PROGRAM: map_message
RULE: --> PenDecoder.map_update
-IN: :type :x_size :y_size :x_origin :y_origin :resolution :encoded_map
+IN: :type :order :x_size :y_size :x_origin :y_origin :resolution :encoded_map
OUT:
PROGRAM: trader_message
Modified: trunk/TeamTalk/Configurations/DesktopConfiguration/TeamTalk-hub-desktop-template.pgm
===================================================================
--- trunk/TeamTalk/Configurations/DesktopConfiguration/TeamTalk-hub-desktop-template.pgm 2007-09-25 16:13:35 UTC (rev 791)
+++ trunk/TeamTalk/Configurations/DesktopConfiguration/TeamTalk-hub-desktop-template.pgm 2007-09-27 06:27:53 UTC (rev 792)
@@ -259,8 +259,3 @@
RULE: :voice --> kalliope.add_voice
IN: :name :voice
OUT:
-
-;;PROGRAM: map_message
-;;RULE: --> PenDecoder.map_update
-;;IN: :type :x_size :y_size :resolution :encoded_map
-;;OUT:
Modified: trunk/TeamTalk/Libraries/PrimitiveComm/robot_packet2.cpp
===================================================================
--- trunk/TeamTalk/Libraries/PrimitiveComm/robot_packet2.cpp 2007-09-25 16:13:35 UTC (rev 791)
+++ trunk/TeamTalk/Libraries/PrimitiveComm/robot_packet2.cpp 2007-09-27 06:27:53 UTC (rev 792)
@@ -663,10 +663,10 @@
Boeing::MapMsgEncoding e;
switch(encoding_) {
case MsgMap::RAW:
- e = Boeing::MAP_RLE;
+ e = Boeing::MAP_Y_MAJOR;
break;
case MsgMap::JPEG:
- e = Boeing::MAP_RAW;
+ e = Boeing::MAP_JPEG;
break;
default: throw MalformedPacketException("MsgMap::renderBoeingPacket()", "");
}
Modified: trunk/TeamTalk/Libraries/boeingLib/boeing/boeing_map_packet.cc
===================================================================
--- trunk/TeamTalk/Libraries/boeingLib/boeing/boeing_map_packet.cc 2007-09-25 16:13:35 UTC (rev 791)
+++ trunk/TeamTalk/Libraries/boeingLib/boeing/boeing_map_packet.cc 2007-09-27 06:27:53 UTC (rev 792)
@@ -5,6 +5,16 @@
using namespace Boeing;
using namespace std;
+MapCodingException::MapCodingException() throw() {}
+
+MapCodingException::MapCodingException(const string& x) throw(): error_str(x) {}
+
+MapCodingException::~MapCodingException() throw() {}
+
+const char* MapCodingException::what() const throw() {
+ return error_str.c_str();
+}
+
void MsgMap::MsgMapFactory(MapMsgEncoding encoding,
MsgMap *buf, const void* raw_map, int raw_map_size,
short invoice, int seq,
@@ -31,61 +41,87 @@
int MsgMap::getSize() const {
switch(encoding) {
- case MAP_RLE:
- return sizeof(MsgMap)+sizeof(int)*(array_length-1);
- case MAP_RAW:
- case MAP_JPEG:
- return sizeof(MsgMap)-sizeof(int)+array_length;
- default:
- cerr << "unknown encoding" << endl;
+ case MAP_Y_MAJOR:
+ case MAP_X_MAJOR:
+ return sizeof(MsgMap)+sizeof(int)*(array_length-1);
+ case MAP_JPEG:
+ return sizeof(MsgMap)-sizeof(int)+array_length;
+ default:
+ cerr << "unknown encoding" << endl;
}
return 0;
}
int MsgMap::getBuffSize() const {
switch(encoding) {
- case MAP_RAW:
- case MAP_JPEG:
- return array_length;
- case MAP_RLE:
- return x*y;
- default: cerr << "unknown encoding" << endl;
+ case MAP_JPEG:
+ return array_length;
+ case MAP_Y_MAJOR:
+ case MAP_X_MAJOR:
+ return x*y;
+ default: cerr << "unknown encoding" << endl;
}
return 0;
}
-void MsgMap::unencodeMap(MapMsgEncoding encoding,
- unsigned char *raw_map,
- const unsigned char* compressed_map,
- int compressed_map_size) {
- switch(encoding) {
- case MAP_RAW:
+bool MsgMap::decode(MapMsgEncoding raw_map_encoding,
+ unsigned char *raw_map) const {
+ switch(raw_map_encoding) {
+ case MAP_JPEG:
+ switch(encoding) {
case MAP_JPEG:
- memcpy(raw_map, compressed_map, compressed_map_size);
- break;
- case MAP_RLE:
- unencodeMap_RLE(raw_map, compressed_map, compressed_map_size);
- break;
- default: cerr << "unknown encoding" << endl;
+ memcpy(raw_map, map, array_length);
+ return true;
+ default: throw MapCodingException("JPEG unencoding not supported");
+ }
+ break;
+ case MAP_Y_MAJOR:
+ switch(encoding) {
+ case MAP_JPEG: throw MapCodingException("JPEG encoding not supported");
+ case MAP_Y_MAJOR: return unencodeMap_RLE(raw_map);
+ case MAP_X_MAJOR: return unencodeMap_RLE_trans(x, y, raw_map);
+ default: throw MapCodingException("unknown encoding in message");
+ }
+ break;
+ case MAP_X_MAJOR:
+ switch(encoding) {
+ case MAP_JPEG: throw MapCodingException("JPEG encoding not supported");
+ case MAP_Y_MAJOR: return unencodeMap_RLE_trans(y, x, raw_map);
+ case MAP_X_MAJOR: return unencodeMap_RLE(raw_map);
+ default: throw MapCodingException("unknown encoding in message");
+ }
+ break;
+ default: throw MapCodingException("unknown encoding");
}
}
-void MsgMap::decode(unsigned char *raw_map) const {
- unencodeMap((MapMsgEncoding)encoding, raw_map, (const unsigned char *)map, array_length);
+bool MsgMap::unencodeMap_RLE(unsigned char *raw_map) const {
+ bool changed = false;
+
+ for(int i=0; i<array_length; i++) {
+ int run_length = (map[i]&0xFFFFFF00)>>8;
+ char run_value = (char) (map[i]&0x000000FF);
+ memset(&(raw_map[i]),run_value,run_length);
+ changed |= (run_value != MAP_CELL_UNCHANGED);
+ }
+ return changed;
}
-void MsgMap::unencodeMap_RLE(unsigned char *raw_map,
- const unsigned char* compressed_map,
- int compressed_map_size) {
+bool MsgMap::unencodeMap_RLE_trans(int old_major_dim,
+ int new_major_dim,
+ unsigned char *raw_map) const {
+ bool changed = false;
int j=0, k=0;
- for(int i=0; i<compressed_map_size; i++) {
- int run_length = (compressed_map[i]&0xFFFFFF00)>>8;
- char run_value = (char) (compressed_map[i]&0x000000FF);
- for(; j<k+run_length; j++) {
- raw_map[j] = run_value;
- }
+
+ for(int i=0; i<array_length; i++) {
+ int run_length = (map[i]&0xFFFFFF00)>>8;
+ char run_value = (char) (map[i]&0x000000FF);
+ for(; j<k+run_length; j++)
+ raw_map[j%old_major_dim*new_major_dim+j/old_major_dim] = run_value;
k=j;
+ changed |= (run_value != MAP_CELL_UNCHANGED);
}
+ return changed;
}
int MsgMap::encodeMap(MapMsgEncoding encoding,
@@ -93,22 +129,22 @@
const unsigned char* raw_map,
unsigned int raw_map_size) {
switch (encoding) {
- case MAP_RAW:
- case MAP_JPEG:
- memcpy(buf, raw_map, raw_map_size);
- return raw_map_size;
- case MAP_RLE:
- return encodeMap_RLE(buf, raw_map, raw_map_size);
- default:
- cerr << "unknown encoding: " << encoding << endl;
+ case MAP_JPEG:
+ memcpy(buf, raw_map, raw_map_size);
+ return raw_map_size;
+ case MAP_Y_MAJOR:
+ case MAP_X_MAJOR:
+ return encodeMap_RLE(buf, raw_map, raw_map_size);
+ default:
+ cerr << "unknown encoding: " << encoding << endl;
}
return -1;
}
//encode, returns the compressed map size
int MsgMap::encodeMap_RLE(unsigned char *compressed_map,
- const unsigned char* raw_map,
- unsigned int raw_map_size) {
+ const unsigned char* raw_map,
+ unsigned int raw_map_size) {
if(!raw_map_size) { //odd, map has no area
return 0;
}
Modified: trunk/TeamTalk/Libraries/boeingLib/boeing/boeing_map_packet.h
===================================================================
--- trunk/TeamTalk/Libraries/boeingLib/boeing/boeing_map_packet.h 2007-09-25 16:13:35 UTC (rev 791)
+++ trunk/TeamTalk/Libraries/boeingLib/boeing/boeing_map_packet.h 2007-09-27 06:27:53 UTC (rev 792)
@@ -6,6 +6,8 @@
#define __BOEING_MAP_PACKET_H__
#include <string.h>
+#include <exception>
+#include <string>
#include "boeing_net.h"
@@ -24,23 +26,45 @@
#endif
#endif
- //===================================================-
+ //================================================
// Message ID's
// we combine all of them into one
enum MapMsgID { MAP_SUBSCRIBE=0,
- MAP_KEEPALIVE,
+ MAP_KEEPALIVE,
MAP_UNSUBSCRIBE,
MAP_ACK,
MAP_FULL,
- MAP_DIFF
+ MAP_DIFF
};
- enum MapMsgEncoding { MAP_RAW=0, // a raster of bytes
- MAP_RLE, // a run-length enoding
+ //================================================
+ // Other enums
+
+ enum MapMsgEncoding { MAP_Y_MAJOR=0, // a y-major raster of bytes
+ MAP_X_MAJOR, // an x-major raster of bytes
MAP_JPEG // a jpeg
};
+ //interpretation of raw_map cell chars
+
+ enum MapCellType { MAP_CELL_UNCHANGED=0,
+ MAP_CELL_CLEAR= 0x20,
+ MAP_CELL_UNKNOWN= 0x40,
+ MAP_CELL_OCCUPIED= 0xff };
+
+ //================================================
+ // Packet manipulation exceptions
+
+ class MapCodingException: public std::exception {
+ std::string error_str;
+ public:
+ MapCodingException() throw();
+ MapCodingException(const std::string& x) throw();
+ ~MapCodingException() throw();
+ const char* what() const throw();
+ };
+
//=== Commands for Map ===========================
// these are the commands received by the server,
// and sent by the client
@@ -70,7 +94,8 @@
//
// map messages: Note these are variable length!!!
- struct MsgMap : public MsgHeader {
+ class MsgMap : public MsgHeader {
+ public:
short invoice;
short encoding;
int seq;
@@ -82,13 +107,24 @@
static const int MAX_RUNLENGTH = 1<<23;
+ protected:
+ bool unencodeMap_RLE(unsigned char *raw_map) const;
+ bool unencodeMap_RLE_trans(int old_major_dim,
+ int new_major_dim,
+ unsigned char *raw_map) const;
+ //void unencodeMap_JPEG(unsigned char *raw_map,
+ // unsigned const char
+ // *compressed_map,
+ // int compressed_map_size);
+ static int encodeMap_RLE(unsigned char *buf,
+ const unsigned char* rawmap,
+ unsigned int j);
+ static int encodeMap_JPEG(unsigned char *buf,
+ const unsigned char* rawmap,
+ unsigned int j);
+
+ public:
//puts a MsgMap into the buf
- //interpretation of raw_map cell chars
- // 0x00 -> unchanged
- // 0x20 -> clear
- // 0x40 -> unseen/unknown
- // 0xff -> occupied
- //raw map is interpreted to be left-handed and height-major
static void MsgMapFactory(MapMsgEncoding encodingType,
MsgMap *buf, const void* raw_map, int raw_map_size,
short invoice, int seq,
@@ -105,20 +141,10 @@
int getBuffSize() const; //returns the minimum size of the required decode buf (in bytes)
//decode
- static void unencodeMap(MapMsgEncoding encoding,
- unsigned char *raw_map, const unsigned char* compressed_map, int compressed_map_size);
- static void unencodeMap_RLE(unsigned char *raw_map,
- unsigned const char* compressed_map,
- int compressed_map_size);
- static void unencodeMap_JPEG(unsigned char *raw_map,
- unsigned const char *compressed_map, int compressed_map_size);
- void decode(unsigned char *raw_map) const;
+ bool decode(MapMsgEncoding raw_map_encoding, unsigned char *raw_map) const;
//encode
static int encodeMap(MapMsgEncoding encoding, unsigned char *buf, const unsigned char* rawmap, unsigned int j);
- static int encodeMap_RLE(unsigned char *buf, const unsigned char* rawmap, unsigned int j);
- static int encodeMap_JPEG(unsigned char *buf, const unsigned char* rawmap, unsigned int j);
-
} PACKED;
union MsgMapClient {
More information about the TeamTalk-developers
mailing list