[TeamTalk 21]: [558] TeamTalk/Agents/TeamTalkSimulator: Changes to support latest version of PrimitiveComm.
tk@edam.speech.cs.cmu.edu
tk at edam.speech.cs.cmu.edu
Mon Dec 11 03:10:22 EST 2006
An HTML attachment was scrubbed...
URL: http://mailman.srv.cs.cmu.edu/pipermail/teamtalk-developers/attachments/20061211/35fa865a/attachment.html
-------------- next part --------------
Modified: TeamTalk/Agents/TeamTalkSimulator/TeamTalkSimulator.cc
===================================================================
--- TeamTalk/Agents/TeamTalkSimulator/TeamTalkSimulator.cc 2006-12-11 08:04:39 UTC (rev 557)
+++ TeamTalk/Agents/TeamTalkSimulator/TeamTalkSimulator.cc 2006-12-11 08:10:22 UTC (rev 558)
@@ -13,6 +13,7 @@
#include <getopt.h>
#include <pthread.h>
#include <signal.h>
+#include <unistd.h>
using namespace std;
@@ -60,21 +61,21 @@
/****************** MAP STUFF *********************/
static unsigned char* conveyedMap;
-static unsigned char* currentMap;
+static unsigned char* currentMap = (unsigned char*)malloc(500000);
static unsigned char* diffMap;
static int mapWidth, mapHeight;
static int mapXOffset, mapYOffset;
+static Boeing::MsgMap* msgMap = (Boeing::MsgMap*)malloc(500000);
-
void readMapFile() {
//we're going to do this a lot, so just open once
}
-void sendFullMap(Boeing::MapServer *m) {
+void sendFullMap(Boeing::MapServer *mserver) {
readMapFile();
Boeing::MsgMap::MsgMapFactory(Boeing::MAP_RLE,
- msgMap, mapData, mapWidth*mapHeight,
+ msgMap, currentMap, mapWidth*mapHeight,
0, 0, mapWidth, mapHeight);
cerr << "raw: " << msgMap->map[0]
<< " rl: " << ((msgMap->map[0]&0xFFFFFF00)>>8)
@@ -82,10 +83,10 @@
mserver->sendFullMap(msgMap);
}
-void sendDiffMap(Boeing::MapServer *m) {
+void sendDiffMap(Boeing::MapServer *mserver) {
readMapFile();
Boeing::MsgMap::MsgMapFactory(Boeing::MAP_RLE,
- msgMap, diff_map,
+ msgMap, diffMap, mapWidth*mapHeight,
0, 0, mapWidth, mapHeight);
cerr << "raw: " << msgMap->map[0]
<< " rl: " << ((msgMap->map[0]&0xFFFFFF00)>>8)
@@ -97,13 +98,13 @@
bool subscribed = false;
int updatePeriod = 1;
- Boeing::MapServer *mserver = (Boeing::MapServer *) m;
+ Boeing::MapServer *mserver = (Boeing::MapServer*)m;
for(;;) {
const Boeing::MsgMapServer *msg = mserver->getNextMessage();
if(msg) {
switch(msg->hdr.type) {
case Boeing::MAP_SUBSCRIBE:
- sendFullMap(m);
+ sendFullMap(mserver);
subscribed = true;
break;
case Boeing::MAP_KEEPALIVE:
@@ -117,7 +118,7 @@
cerr << "unknown or unhandled msgtype: " << msg->hdr.type << endl;
}
} else {
- if(subscribed) sendDiffMap(m);
+ if(subscribed) sendDiffMap(mserver);
sleep(updatePeriod);
}
}
@@ -164,12 +165,11 @@
void readMapData() {
ifstream f("stubdb.txt");
f >> mapWidth >> mapHeight;
- mapData = new unsigned char[mapWidth*mapHeight];
for(int j=0; j<mapHeight; j++) {
for(int i=0; i<mapWidth; i++) {
int n;
f >> n;
- mapData[j*mapWidth+i] = (unsigned char)n;
+ currentMap[j*mapWidth+i] = (unsigned char)n;
}
}
}
Modified: TeamTalk/Agents/TeamTalkSimulator/imageClient.h
===================================================================
--- TeamTalk/Agents/TeamTalkSimulator/imageClient.h 2006-12-11 08:04:39 UTC (rev 557)
+++ TeamTalk/Agents/TeamTalkSimulator/imageClient.h 2006-12-11 08:10:22 UTC (rev 558)
@@ -5,6 +5,12 @@
#include <utils.h>
+#ifndef WIN32
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#endif
+
class ImageClient {
protected:
int sockfd;
Modified: TeamTalk/Agents/TeamTalkSimulator/robot.cc
===================================================================
--- TeamTalk/Agents/TeamTalkSimulator/robot.cc 2006-12-11 08:04:39 UTC (rev 557)
+++ TeamTalk/Agents/TeamTalkSimulator/robot.cc 2006-12-11 08:10:22 UTC (rev 558)
@@ -6,6 +6,7 @@
#include <sstream>
#include <signal.h>
+#include <unistd.h>
#define DEBUG 1
#include "robot.h"
@@ -25,10 +26,6 @@
using namespace std;
-#ifndef PI
-const double PI(4.0 * atan2(1.0, 1.0));
-#endif
-
const unsigned int shift = 6;
const unsigned int mask = ~0U << (sizeof(unsigned int)*8 - shift);
@@ -53,7 +50,7 @@
int Robot::initVehMobPLNmlBuffers(char *config_file, int bufnum)
{
- print_debugln("cleaning up nmlbuffers");
+ debug << "cleaning up nmlbuffers" << endl;
cleanupVehMobPLNmlBuffers();
{
@@ -229,7 +226,7 @@
me->vehMobPLSetBuf->read();
me->primMobJAStatBuf->read();
- print_debugln("command initialization");
+ debug << "command initialization" << endl;
VehMobPLCmdInit init = VehMobPLCmdInit();
init.serial_number = me->statP->echo_serial_number+1;
me->vehMobPLCmdBuf->write(&init);
@@ -281,16 +278,20 @@
//strcpy(dump.fileName, "/tmp/VehMobPLDispOutput.ppm");
//me->vehMobPLCfgBuf->write(&dump);
- print_debugln("entering for loop");
+ debug << "entering for loop" << endl;
//EnterCriticalSection(&me->CriticalSection);
for(int i;;i++) {
//LeaveCriticalSection(&me->CriticalSection);
do {
const Boeing::MsgCmd *bmsg;
- //if(me->server_->isConnected()) print_debug('.');
while(me->server_->isConnected() &&
(bmsg = me->server_->getNextMessage())) {
- Msg* msg = Msg::interpret(bmsg->buff);
+ Msg* msg = Msg::interpretBoeingPacket(string(bmsg->buff, 1000));
+ if(!msg) {
+ warn << "got NULL message" << endl;
+ break;
+ }
+ debug << "got message: " << msg->render() << endl;
//if(!dynamic_cast<MsgReqLocationPP*>(msg)) {
// print_debug("got message: "); print_debugln(msg->render());
//}
@@ -400,26 +401,26 @@
}
// initial allocate all the buffers
- print_debugln("initial allocation of the buffers");
+ debug << "initial allocation of the buffers" << endl;
if(initVehMobPLNmlBuffers(NML_FILE, sim_index)) {
cerr << "can't allocate NML buffers" << endl;
}
// set up our convenient pointer to status and settings
- print_debugln("set up convenient pointers");
+ debug << "set up convenient pointers" << endl;
statP = (VehMobPLStat *) vehMobPLStatBuf->get_address();
primStatP = (PrimMobJAStat *) primMobJAStatBuf->get_address();
setP = (VehMobPLSet *) vehMobPLSetBuf->get_address();
- print_debugln("new robotserver");
+ debug << "new robotserver" << endl;
server_ = new Boeing::RobotServer();
- print_debugln("opening port");
+ debug << "opening port" << endl;
if (port) server_->open(port);
else server_->open();
- print_debugln("starting reading thread");
+ debug << "starting reading thread" << endl;
pthread_t simulation_thread;
pthread_create(&simulation_thread, NULL, simulate, (void *)this);
- print_debugln("done with robot constructor");
+ debug << "done with robot constructor" << endl;
}
Robot::~Robot() {
@@ -438,14 +439,14 @@
primMobJAStatBuf->read();
Msg* temp_message;
- if(const MsgCmdActionPP* action = dynamic_cast<const MsgCmdActionPP*>(msgp)) {
- //print_debugln("got an action");
- //need to translate txt-based message into typed message
- temp_message = Msg::interpretPlayAction(*action->content());
- } else {
- temp_message = msgp->clone();
- }
- if(const MsgMapReqPP* req_image = dynamic_cast<const MsgMapReqPP*>(temp_message)) {
+ //if(const MsgCmdActionPP* action = dynamic_cast<const MsgCmdActionPP*>(msgp)) {
+ // //print_debugln("got an action");
+ // //need to translate txt-based message into typed message
+ // temp_message = Msg::interpretPlayAction(*action->content());
+ //} else {
+ // temp_message = msgp->clone();
+ //}
+ if(const MsgMapReq* req_image = dynamic_cast<const MsgMapReq*>(temp_message)) {
//print_debugln("got an req image");
//get the jpeg from the usarsim video socket
const unsigned char* image;
@@ -468,67 +469,52 @@
//print_debugln("done with req image");
return;
}
- if(dynamic_cast<const MsgReqLocationPP*>(temp_message)) {
+ if(dynamic_cast<const MsgReqLocation*>(temp_message)) {
//print_debugln("got an req location");
server_->sendLocation(getX(), -getY(), canonical_angle(-getYaw()), true);
delete temp_message;
return;
}
//EnterCriticalSection(&CriticalSection);
- if(const MsgCmdGoToRelativePP* goto_relative = dynamic_cast<const MsgCmdGoToRelativePP*>(temp_message)) {
- cout << "got: " << *goto_relative << endl;
- float m_x = goto_relative->content()->x;
- float m_y = -goto_relative->content()->y;
- float dir = getYaw()+atan2(m_y, m_x);
- float len = sqrt(m_x*m_x+m_y*m_y);
- VehMobPLCmdMoveWaypoint move = VehMobPLCmdMoveWaypoint();
- move.p.x = getX()+cos(dir)*len;
- move.p.y = getY()+sin(dir)*len;
- //cerr << "m_x: " << m_x << " m_y: " << m_y << " yaw: " << getYaw()
- // << " curdir: " << atan2(m_y, m_x) << " dir: " << dir
- // << " len: " << len << " c_x: " << getX() << " c_y: " << getY()
- // << " movex: " << move.p.x << " movey: " << move.p.y << endl;
- move.serial_number = statP->echo_serial_number+1;
- move.isGlobal = true;
- move.neighborhood = 1;
- move.startTime = 0;
- vehMobPLCmdBuf->write(&move);
+ if(const MsgCmdGoTo* goTo = dynamic_cast<const MsgCmdGoTo*>(temp_message)) {
+ cout << "got: " << goTo << endl;
+ Point<float> vec(goTo->getX(), goTo->getY());
+ if(goTo->useAngle() && !vec) {
+ PrimMobJACmdRotate rotate = PrimMobJACmdRotate();
+ float r = -getYaw();
+ rotate.theta = canonical_angle(-(r+goTo->getAngle()));
+ rotate.thetaTol = PI/20; //9 degrees tolerance
+ // always make it a new command
+ rotate.serial_number = primStatP->echo_serial_number + 1;
+ primMobJACmdBuf->write(&rotate);
+ } else {
+ Point<float> loc(getX(), getY());
+ Point<float> destination;
+ if(goTo->isAbsolute()) {
+ destination = vec;
+ } else {
+ destination =
+ loc + Point<float>::Polar(vec.length(), getYaw()+vec.angle());
+ }
+ VehMobPLCmdMoveWaypoint move = VehMobPLCmdMoveWaypoint();
+ move.p.x = destination.x;
+ move.p.y = destination.y;
+ move.serial_number = statP->echo_serial_number+1;
+ move.isGlobal = true;
+ move.neighborhood = 1;
+ move.startTime = 0;
+ vehMobPLCmdBuf->write(&move);
+ }
delete temp_message;
return;
- } else if(const MsgCmdHaltPP* halt = dynamic_cast<const MsgCmdHaltPP*>(temp_message)) {
- cout << "got: " << *halt << endl;
+ } else if(const MsgCmdHalt* halt = dynamic_cast<const MsgCmdHalt*>(temp_message)) {
+ cout << "got: " << halt << endl;
action = WAITING;
delete temp_message;
return;
- } else if(const MsgCmdGoToGlobalPP* goto_global = dynamic_cast<const MsgCmdGoToGlobalPP*>(temp_message)) {
- cout << "got: " << *goto_global << endl;
- //setGoal(goto_global->content()->x, goto_global->content()->y);
+ } else if(const MsgCmdHome* home = dynamic_cast<const MsgCmdHome*>(temp_message)) {
+ cout << "got: " << home << endl;
VehMobPLCmdMoveWaypoint move = VehMobPLCmdMoveWaypoint();
- move.p.x = goto_global->content()->x;
- move.p.y = -goto_global->content()->y;
- move.serial_number = statP->echo_serial_number+1;
- move.isGlobal = true;
- move.neighborhood = 1;
- move.startTime = 0;
- vehMobPLCmdBuf->write(&move);
- delete temp_message;
- return;
- } else if(const MsgCmdTurnRelativePP* turn = dynamic_cast<const MsgCmdTurnRelativePP*>(temp_message)) {
- cout << "got: " << *turn << endl;
- PrimMobJACmdRotate rotate = PrimMobJACmdRotate();
- float r = -getYaw();
- rotate.theta = canonical_angle(-(r+turn->content()->angle));
- rotate.thetaTol = PI/20; //9 degrees tolerance
- // always make it a new command
- rotate.serial_number = primStatP->echo_serial_number + 1;
- primMobJACmdBuf->write(&rotate);
- //goal_r = canonical_angle(r+turn->content()->angle);
- //action = GO_TO_ANGLE; //will report done when complete
- delete temp_message;
- return;
- } else if(const MsgCmdHomePP* home = dynamic_cast<const MsgCmdHomePP*>(temp_message)) {
- cout << "got: " << *home << endl;
- VehMobPLCmdMoveWaypoint move = VehMobPLCmdMoveWaypoint();
move.p.x = 0;
move.p.y = 0;
move.serial_number = statP->echo_serial_number+1;
@@ -538,19 +524,19 @@
vehMobPLCmdBuf->write(&move);
delete temp_message;
return;
- } else if(const MsgCmdPausePP* pause = dynamic_cast<const MsgCmdPausePP*>(temp_message)) {
- cout << "got: " << *pause << endl;
+ } else if(const MsgCmdPause* pause = dynamic_cast<const MsgCmdPause*>(temp_message)) {
+ cout << "got: " << pause << endl;
if(action != PAUSED) {
paused_action = action;
action = PAUSED;
}
- } else if(const MsgCmdResumePP* resume = dynamic_cast<const MsgCmdResumePP*>(temp_message)) {
- cout << "got: " << *resume << endl;
+ } else if(const MsgCmdResume* resume = dynamic_cast<const MsgCmdResume*>(temp_message)) {
+ cout << "got: " << resume << endl;
if(action == PAUSED) {
action = paused_action;
}
- } else if(const MsgCmdFollowPP* follow = dynamic_cast<const MsgCmdFollowPP*>(temp_message)) {
- cout << "got: " << *follow << endl;
+ } else if(const MsgCmdFollow* follow = dynamic_cast<const MsgCmdFollow*>(temp_message)) {
+ cout << "got: " << follow << endl;
//setFollow(follow->content()->leader);
}
//LeaveCriticalSection(&CriticalSection);
Modified: TeamTalk/Agents/TeamTalkSimulator/robot.h
===================================================================
--- TeamTalk/Agents/TeamTalkSimulator/robot.h 2006-12-11 08:04:39 UTC (rev 557)
+++ TeamTalk/Agents/TeamTalkSimulator/robot.h 2006-12-11 08:10:22 UTC (rev 558)
@@ -2,7 +2,7 @@
#define ROBOT_H
#include <boeing_robot_server.h>
-#include <robot_packet.hpp>
+#include <robot_packet2.h>
#include <map>
#include <stack>
More information about the TeamTalk-developers
mailing list