[TeamTalk 9]: [546] TeamTalk/Agents: backendstub:
tk@edam.speech.cs.cmu.edu
tk at edam.speech.cs.cmu.edu
Thu Dec 7 14:45:00 EST 2006
An HTML attachment was scrubbed...
URL: http://mailman.srv.cs.cmu.edu/pipermail/teamtalk-developers/attachments/20061207/e32a4c47/attachment-0001.html
-------------- next part --------------
Modified: TeamTalk/Agents/Agents.sln
===================================================================
--- TeamTalk/Agents/Agents.sln 2006-11-09 20:10:38 UTC (rev 545)
+++ TeamTalk/Agents/Agents.sln 2006-12-07 19:44:59 UTC (rev 546)
@@ -35,6 +35,41 @@
EndProjectSection
EndProject
Global
+ GlobalSection(SourceCodeControl) = preSolution
+ SccNumberOfProjects = 8
+ SccProjectUniqueName0 = MultiDecoder\\Audio_Server\\Audio_Server.vcproj
+ SccProjectName0 = \u0022$/CommonAgents-2005/MultiDecoder/Audio_Server\u0022,\u0020ONACAAAA
+ SccLocalPath0 = MultiDecoder\\Audio_Server
+ SccProvider0 = MSSCCI:Microsoft\u0020Visual\u0020SourceSafe
+ SccProjectUniqueName1 = Helios3\\Helios3.vcproj
+ SccProjectName1 = \u0022$/CommonAgents-2005/Helios3\u0022,\u0020ZJACAAAA
+ SccLocalPath1 = Helios3
+ SccProvider1 = MSSCCI:Microsoft\u0020Visual\u0020SourceSafe
+ SccProjectUniqueName2 = Kalliope\\Kalliope.vcproj
+ SccProjectName2 = \u0022$/CommonAgents-2005/Kalliope\u0022,\u0020MLACAAAA
+ SccLocalPath2 = Kalliope
+ SccProvider2 = MSSCCI:Microsoft\u0020Visual\u0020SourceSafe
+ SccProjectUniqueName3 = NlgServer2\\NlgServer2.vcproj
+ SccProjectName3 = \u0022$/CommonAgents-2005/NlgServer2\u0022,\u0020JOACAAAA
+ SccLocalPath3 = NlgServer2
+ SccProvider3 = MSSCCI:Microsoft\u0020Visual\u0020SourceSafe
+ SccProjectUniqueName4 = Phoenix2\\phoenix2.vcproj
+ SccProjectName4 = \u0022$/CommonAgents-2005/Phoenix2\u0022,\u0020APACAAAA
+ SccLocalPath4 = Phoenix2
+ SccProvider4 = MSSCCI:Microsoft\u0020Visual\u0020SourceSafe
+ SccProjectUniqueName5 = ProcessMonitor\\ProcessMonitor.vcproj
+ SccProjectName5 = \u0022$/CommonAgents-2005/ProcessMonitor\u0022,\u0020VPACAAAA
+ SccLocalPath5 = ProcessMonitor
+ SccProvider5 = MSSCCI:Microsoft\u0020Visual\u0020SourceSafe
+ SccProjectUniqueName6 = RavenClaw\\RavenClaw.vcproj
+ SccProjectName6 = \u0022$/CommonAgents-2005/RavenClaw\u0022,\u0020PRACAAAA
+ SccLocalPath6 = RavenClaw
+ SccProvider6 = MSSCCI:Microsoft\u0020Visual\u0020SourceSafe
+ SccProjectUniqueName7 = MultiDecoder\\Sphinx_Engine\\Sphinx_Engine.vcproj
+ SccProjectName7 = \u0022$/CommonAgents-2005/MultiDecoder/Sphinx_Engine\u0022,\u0020BOACAAAA
+ SccLocalPath7 = MultiDecoder\\Sphinx_Engine
+ SccProvider7 = MSSCCI:Microsoft\u0020Visual\u0020SourceSafe
+ EndGlobalSection
GlobalSection(SolutionConfigurationPlatforms) = preSolution
Debug|Win32 = Debug|Win32
Release|Win32 = Release|Win32
@@ -42,8 +77,8 @@
GlobalSection(ProjectConfigurationPlatforms) = postSolution
{C31484B0-179B-432D-AE1E-75FB90591F23}.Debug|Win32.ActiveCfg = Debug|Win32
{C31484B0-179B-432D-AE1E-75FB90591F23}.Debug|Win32.Build.0 = Debug|Win32
- {C31484B0-179B-432D-AE1E-75FB90591F23}.Release|Win32.ActiveCfg = Release|Win32
- {C31484B0-179B-432D-AE1E-75FB90591F23}.Release|Win32.Build.0 = Release|Win32
+ {C31484B0-179B-432D-AE1E-75FB90591F23}.Release|Win32.ActiveCfg = Debug|Win32
+ {C31484B0-179B-432D-AE1E-75FB90591F23}.Release|Win32.Build.0 = Debug|Win32
{93C8F5F8-6C43-4179-9B9F-A31AA6438513}.Debug|Win32.ActiveCfg = Debug|Win32
{93C8F5F8-6C43-4179-9B9F-A31AA6438513}.Debug|Win32.Build.0 = Debug|Win32
{93C8F5F8-6C43-4179-9B9F-A31AA6438513}.Release|Win32.ActiveCfg = Release|Win32
Modified: TeamTalk/Agents/PrimitiveComm/PrimitiveComm.vcproj
===================================================================
--- TeamTalk/Agents/PrimitiveComm/PrimitiveComm.vcproj 2006-11-09 20:10:38 UTC (rev 545)
+++ TeamTalk/Agents/PrimitiveComm/PrimitiveComm.vcproj 2006-12-07 19:44:59 UTC (rev 546)
@@ -184,15 +184,11 @@
>
</File>
<File
- RelativePath=".\geometry.cpp"
- >
- </File>
- <File
RelativePath=".\robot_client.cpp"
>
</File>
<File
- RelativePath=".\robot_packet.cpp"
+ RelativePath=".\robot_packet2.cpp"
>
</File>
<File
@@ -211,6 +207,10 @@
RelativePath="..\boeingLib\coralshared\win32dep.cc"
>
</File>
+ <File
+ RelativePath=".\win_netutils.cpp"
+ >
+ </File>
</Filter>
<Filter
Name="Header Files"
@@ -230,6 +230,10 @@
>
</File>
<File
+ RelativePath="..\boeingLib\boeing\boeing_net.h"
+ >
+ </File>
+ <File
RelativePath="..\boeingLib\boeing\boeing_robot_client.h"
>
</File>
@@ -262,11 +266,11 @@
>
</File>
<File
- RelativePath=".\robot_packet.h"
+ RelativePath=".\robot_packet.hpp"
>
</File>
<File
- RelativePath=".\robot_packet.hpp"
+ RelativePath=".\robot_packet2.h"
>
</File>
<File
@@ -289,6 +293,10 @@
RelativePath="..\boeingLib\coralshared\win32dep.h"
>
</File>
+ <File
+ RelativePath=".\win_netutils.h"
+ >
+ </File>
</Filter>
<Filter
Name="Resource Files"
Deleted: TeamTalk/Agents/PrimitiveComm/geometry.cpp
===================================================================
--- TeamTalk/Agents/PrimitiveComm/geometry.cpp 2006-11-09 20:10:38 UTC (rev 545)
+++ TeamTalk/Agents/PrimitiveComm/geometry.cpp 2006-12-07 19:44:59 UTC (rev 546)
@@ -1,130 +0,0 @@
-#include <cmath>
-#include <iostream>
-
-#include "geometry.h"
-#include "utils.h"
-
-using namespace geometry;
-
-Vector::Vector() {};
-Vector::Vector(double X, double Y) : x(X), y(Y) {};
-Vector Vector::rotate(double rads) {
- double r = sqrt(x*x + y*y);
- double theta = atan2(y, x);
- x = r*cos(theta+rads);
- y = r*sin(theta+rads);
- return *this;
-}
-
-double Vector::length() const {
- return sqrt(x*x+y*y);
-};
-
-double Vector::angle() const {
- return atan2(y, x);
-};
-
-ostream& operator<<(ostream& out, const Vector& p) {
- return out << '[' << p.x << ',' << p.y << ']';
-};
-
-istream& operator>>(istream& in, Vector& p) {
- //expecting "\s*(\s*f\s+f\s*)" or "\s*f\s+f";
-
- bool has_paren = false;
-
- while(!in.eof()) {
- char c = in.peek();
- if (isspace(c)) {
- in.ignore();
- } else if (c == '(') {
- in.ignore();
- has_paren = true;
- break;
- } else break;
- }
-
- in >> p.x >> p.y;
-
- if(has_paren) {
- while(!in.eof()) {
- char c = in.peek();
- if (isspace(c)) {
- in.ignore();
- } else if (c == ')') {
- in.ignore();
- break;
- } else break;
- }
- }
-
- return in;
-};
-
-istream& operator>>(istream& in, geometry::Polygon& l) {
- l.clear();
- geometry::Point p;
- while(in >> p) {
- l.push_back(p);
- }
- return in;
-};
-
-Quadrilateral::Quadrilateral(Vector lower_left, Vector upper_right) : Polygon(4) {
- // axis-aligned quadrilateral
- at(0) = Point(lower_left.x, lower_left.y);
- at(1) = Point(lower_left.x, upper_right.y);
- at(2) = Point(upper_right.x, upper_right.y);
- at(3) = Point(upper_right.x, lower_left.y);
-};
-
-Quadrilateral::Quadrilateral(double x1, double y1, double x2, double y2) : Polygon(4) {
- at(0) = Point(MIN(x1, x2), MIN(y1, y2));
- at(1) = Point(MIN(x1, x2), MAX(y1, y2));
- at(2) = Point(MAX(x1, x2), MAX(y1, y2));
- at(3) = Point(MAX(x1, x2), MIN(y1, y2));
-};
-
-Quadrilateral::Quadrilateral(Vector p1, Vector p2, Vector p3, Vector p4) : Polygon(4) {
- at(0) = Point(p1.x, p1.y);
- at(1) = Point(p2.x, p2.y);
- at(2) = Point(p3.x, p3.y);
- at(3) = Point(p4.x, p4.y);
-};
-
-Quadrilateral::operator geometry::Polygon() const {
- return *this;
-};
-
-Point Quadrilateral::ll() const {
- Point retval = at(0);
- for(int i=1; i<4; i++) {
- if(at(i).x < retval.x || at(i).y < retval.y) retval = at(i);
- }
- return retval;
-};
-
-Point Quadrilateral::ul() const {
- Point retval = at(0);
- for(int i=1; i<4; i++) {
- if(at(i).x > retval.x || at(i).y < retval.y) retval = at(i);
- }
- return retval;
-};
-
-Point Quadrilateral::ur() const {
- Point retval = at(0);
- for(int i=1; i<4; i++) {
- if(at(i).x > retval.x || at(i).y > retval.y) retval = at(i);
- }
- return retval;
-};
-
-Point Quadrilateral::lr() const {
- Point retval = at(0);
- for(int i=1; i<4; i++) {
- if(at(i).x < retval.x || at(i).y > retval.y) retval = at(i);
- }
- return retval;
-};
-
Modified: TeamTalk/Agents/PrimitiveComm/geometry.h
===================================================================
--- TeamTalk/Agents/PrimitiveComm/geometry.h 2006-11-09 20:10:38 UTC (rev 545)
+++ TeamTalk/Agents/PrimitiveComm/geometry.h 2006-12-07 19:44:59 UTC (rev 546)
@@ -3,45 +3,122 @@
#include <vector>
+#include "utils.h"
+
using namespace std;
namespace geometry {
-struct Vector {
- double x, y;
- Vector();
- Vector(double x, double y);
- Vector rotate(double rads);
- double length() const;
- double angle() const;
+template<typename T> struct Point {
+ T x, y;
+ Point<T>() : x(0), y(0) {};
+ Point<T>(T X, T Y) : x(X), y(Y) {};
+ template<typename L> static Point<T> Polar(L length, double angle) {
+ return Point<T>((T)(cos(angle)*length), (T)(sin(angle)*length));
+ }
+ Point<T> rotate(double rads) {
+ double r = sqrt(x*x + y*y);
+ double theta = atan2(y, x);
+ x = (T) (r*cos(theta+rads));
+ y = (T) (r*sin(theta+rads));
+ return *this;
+ }
+ double length() const {return sqrt(x*x+y*y);};
+ double angle() const {return atan2(y, x);};
+ Point<T> operator-(const Point& rhs) const {
+ return Point<T>(x-rhs.x, y-rhs.y);
+ }
+ Point<T>& operator-=(const Point& rhs) {
+ x -= rhs.x;
+ y -= rhs.y;
+ return *this;
+ }
+ Point<T> operator+(const Point& rhs) const {
+ return Point<T>(x+rhs.x, y+rhs.y);
+ }
+ Point<T> operator+=(const Point& rhs) {
+ x += rhs.x;
+ y += rhs.y;
+ return *this;
+ }
};
-typedef Vector Point;
+template<typename T> struct Polygon : public vector< Point<T> > {
+ Polygon<T>() : vector< Point<T> >() {};
+ Polygon<T> (int n) : vector< Point<T> >(n) {};
+ Polygon<T>(const Polygon<T>& p) : vector< Point<T> >(p) {};
+};
-struct Polygon : public vector<Point> {
- Polygon() : vector<Point>() {};
- Polygon(int n) : vector<Point>(n) {};
- Polygon(const Polygon& p) : vector<Point>(p) {};
+template<typename T> struct Quadrilateral : public Polygon<T> {
+ Quadrilateral<T>() : Polygon<T>(4) {};
+ Quadrilateral<T>(const Quadrilateral<T>& q) : Polygon<T>(q) {};
+ Quadrilateral<T>(Point<T> lower_left, Point<T> upper_right) : Polygon<T>(4) {
+ // axis-aligned quadrilateral
+ at(0) = Point<T>(lower_left.x, lower_left.y);
+ at(1) = Point<T>(lower_left.x, upper_right.y);
+ at(2) = Point<T>(upper_right.x, upper_right.y);
+ at(3) = Point<T>(upper_right.x, lower_left.y);
+ };
+ Quadrilateral<T>(T x1, T x2, T y1, T y2) : Polygon(4) {
+ at(0) = Point<T>(MIN(x1, x2), MIN(y1, y2));
+ at(1) = Point<T>(MIN(x1, x2), MAX(y1, y2));
+ at(2) = Point<T>(MAX(x1, x2), MAX(y1, y2));
+ at(3) = Point<T>(MAX(x1, x2), MIN(y1, y2));
+ };
+ Quadrilateral<T>(Point<T> p1, Point<T> p2, Point<T> p3, Point<T> p4): Polygon(4) {
+ at(0) = p1; at(1) = p2; at(2) = p3; at(3) = p4;
+ };
+ operator Polygon<T>() const {return *this;}
+ Point<T> ll() const {
+ Point<T> retval = at(0);
+ for(int i=1; i<4; i++) {
+ if(at(i).x < retval.x || at(i).y < retval.y) retval = at(i);
+ }
+ return retval;
+ };
+ Point<T> ul() const {
+ Point<T> retval = at(0);
+ for(int i=1; i<4; i++) {
+ if(at(i).x > retval.x || at(i).y < retval.y) retval = at(i);
+ }
+ return retval;
+ };
+ Point<T> ur() const {
+ Point<T> retval = at(0);
+ for(int i=1; i<4; i++) {
+ if(at(i).x > retval.x || at(i).y > retval.y) retval = at(i);
+ }
+ return retval;
+ };
+ Point<T> lr() const {
+ Point<T> retval = at(0);
+ for(int i=1; i<4; i++) {
+ if(at(i).x < retval.x || at(i).y > retval.y) retval = at(i);
+ }
+ return retval;
+ };
};
-struct Quadrilateral : public Polygon {
- Quadrilateral() : Polygon(4) {};
- Quadrilateral(const Quadrilateral& q) : Polygon(q) {};
- Quadrilateral(Point lower_left, Point upper_right); // axis-aligned
- Quadrilateral(double x1, double x2, double y1, double y2); //another axis-aligned
- Quadrilateral(Point p1, Point p2, Point p3, Point p4); //generic
-
- operator Polygon () const;
- Point ll() const;
- Point ul() const;
- Point ur() const;
- Point lr() const;
+template<typename T> struct Pose {
+ Point<T> loc;
+ T yaw;
+ Pose<T>() : loc(), yaw(0) {}
+ Pose<T>(const Point<T>& _loc, T _yaw) : loc(_loc), yaw(_yaw) {}
+ Pose<T>(T _x, T _y, T _yaw) : loc(_x, _y), yaw(_yaw) {}
};
+} //end namespace geometry
+
+template<typename T>
+ostream& operator<<(ostream& out, const geometry::Point<T>& p) {
+ return out << '(' << p.x << ' ' << p.y << ')';
}
+template<typename T>
+istream& operator>>(istream& in, geometry::Point<T>& p) {
+ if(istreamLookFor(in, '(').fail()) return in;
+ if((in >> p.x).fail()) return in;
+ if((in >> p.y).fail()) return in;
+ return istreamLookFor(in, ')');
+}
-ostream& operator<<(ostream& out, const geometry::Vector& p);
-istream& operator>>(istream& in, geometry::Vector& p);
-istream& operator>>(istream& in, geometry::Polygon& l);
-
#endif //GEOMETRY
Modified: TeamTalk/Agents/PrimitiveComm/robot_client.cpp
===================================================================
--- TeamTalk/Agents/PrimitiveComm/robot_client.cpp 2006-11-09 20:10:38 UTC (rev 545)
+++ TeamTalk/Agents/PrimitiveComm/robot_client.cpp 2006-12-07 19:44:59 UTC (rev 546)
@@ -6,6 +6,7 @@
#include "robot_client.hpp"
#include "utils.h"
+#include "win_netutils.h"
#include <cstdio>
#include <cstring>
#include <ctime>
@@ -27,45 +28,48 @@
using namespace std;
using namespace TeamTalk;
-int inet_pton(int family, const char *strptr, void *addrptr) {
- if(family == AF_INET) {
- struct in_addr in_val;
-#ifdef WIN32
- in_val.S_un.S_addr = inet_addr(strptr);
-#else
- in_val.s_addr = inet_addr(strptr);
-#endif
- memcpy(addrptr, &in_val, sizeof(struct in_addr));
- return 1;
- }
- return -1;
+void RobotClient::connect(const string &loc, const string &voice,
+ const string &safeness, unsigned short port) {
+ voice_ = voice;
+ debug << "opening " << loc << ':' << port << endl;
+ open(loc.c_str(), port);
+ if(spawnback) (*spawnback)(name_, safeness);
}
-const unsigned short RobotClient::PORT = 32788;
+void (*RobotClient::spawnback)(const string&, const string&) = NULL;
+
+RobotClient::RobotClient(const string& name) : name_(name) {}
+
+RobotClient::RobotClient(const std::string& name, const std::string& loc,
+ const std::string& voice, const std::string& safeness, unsigned short port)
+ : name_(name) {
+ connect(loc, voice, safeness, port);
+}
+
+string RobotClient::getName() const {return name_;}
+string RobotClient::getVoice() const {return voice_;}
+
+bool RobotClient::operator<(const RobotClient& rhs) const {
+ return name_ < rhs.name_;
+}
+
+//const unsigned short RobotsClient::PORT = 32788;
//const unsigned int RobotClient::MAXLINE = 0xfff; //maximum size of received message <4k
//unsigned int RobotClient::KEEPALIVEPERIOD = 900; //time between pings 15min
//char* RobotClient::BROADCASTADDR = "192.168.1.255";
-RobotClient::RobotClient(const string& handle, void (*c)(const Msg*), void (*n)(const string&, const string&))
- : handle_(handle), callback_(c), spawnback_(n), callbackClass_(NULL)
+RobotsClient::RobotsClient(const string& handle, RobotCallback* robotCallback)
+ : handle_(handle), robotCallback_(robotCallback)
{
+ srand ((unsigned int)time(NULL));
+ Msg::taskIDCounter = rand();
initializeSocketLayer();
getMyAddress();
- //connectOpTrader();
serverThread_ = spawnServer();
}
-RobotClient::RobotClient(const string& handle, CallBackClass* c)
- : handle_(handle), callback_(NULL), callbackClass_(c)
+RobotsClient::~RobotsClient()
{
- initializeSocketLayer();
- getMyAddress();
- //connectOpTrader();
- serverThread_ = spawnServer();
-}
-
-RobotClient::~RobotClient()
-{
// issue: close recieve thread somehow?
#ifdef WIN32
@@ -73,47 +77,13 @@
#endif
}
-void RobotClient::initializeSocketLayer() {
+void RobotsClient::initializeSocketLayer() {
#ifdef WIN32
- {
- WORD wVersionRequested;
- WSADATA wsaData;
- int err;
- wVersionRequested = MAKEWORD( 2, 2 );
- err = WSAStartup( wVersionRequested, &wsaData );
- switch(err) {
- case 0: break; //successful
- case WSASYSNOTREADY:
- throw RobotClientException("Indicates that the underlying network subsystem is not ready for network communication.");
- case WSAVERNOTSUPPORTED:
- throw RobotClientException("The version of Windows Sockets support requested is not provided by this particular Windows Sockets implementation.");
- case WSAEINPROGRESS:
- throw RobotClientException("A blocking Windows Sockets 1.1 operation is in progress.");
- case WSAEPROCLIM:
- throw RobotClientException("Limit on the number of tasks supported by the Windows Sockets implementation has been reached.");
- case WSAEFAULT:
- throw RobotClientException("The lpWSAData is not a valid pointer.");
- default:
- throw RobotClientException("unknown error starting Windows sockets.");
- }
- }
+ initializeSockets();
#endif
}
-Boeing::RobotClient* RobotClient::find(const string& s) {
- map<string, Boeing::RobotClient*>::iterator i = robotAddress.find(s);
- if(i == robotAddress.end()) {
- cerr << "didn't find '" << s << "' in ";
- for(map<string, Boeing::RobotClient*>::iterator i = robotAddress.begin(); i != robotAddress.end(); i++) {
- cerr << "--" << i->first << "-- ";
- }
- cerr << endl;
- return NULL;
- }
- return i->second;
-}
-
-void RobotClient::getMyAddress() {
+void RobotsClient::getMyAddress() {
gethostname(hostname, Boeing::SADDR_LENGTH);
struct hostent *Host = gethostbyname(hostname);
in_addr me;
@@ -122,14 +92,12 @@
}
#ifdef WIN32
-HANDLE RobotClient::spawnServer() {
+HANDLE RobotsClient::spawnServer() {
#else
-pthread_t RobotClient::spawnServer() {
+pthread_t RobotsClient::spawnServer() {
#endif
//spawn server
- //cerr << "about to spawn server" << endl;
#ifdef WIN32
- //_beginthread(keepalive, 0, (void *) this);
return (HANDLE)_beginthread(readMessages, 0, (void *) this);
#else
pthread_t readingThread, keepaliveThread;
@@ -139,101 +107,45 @@
readMessages,
(void *) this
);
- pthread_create(
- &keepaliveThread,
- pthread_attr_default,
- keepalive,
- (void *) this
- );
return readingThread;
#endif
}
-bool RobotClient::addRobot(const string& name,
- const string& loc,
- const string& voice,
- const string& safeness,
- unsigned short port) {
-
- if(!port) port = PORT;
- robotAddress[name] = new Boeing::RobotClient();
- robotAddress[name]->open(loc.c_str(), port);
- robotVoice_[name] = voice;
- if(spawnback_) (*spawnback_)(name, safeness);
- //fixme
+//returns false if a robot by that name already exists
+//else it creates that robot client and inserts it
+bool RobotsClient::addRobot(const string& name, const string& loc,
+ const string& voice, const string& safeness,
+ unsigned short port)
+{
+ if(find(name)) return false;
+ insert(RobotClient(name, loc, voice, safeness, port));
return true;
}
-//adds undiscoverable robots from a file
-void RobotClient::addRobotsFile(const string& name,
- Boeing::TraderClient *tclient,
- Boeing::MapClient *mclient) {
- cerr << "about to add robots" << endl;
- ifstream Frobotips(name.c_str());
- if(!Frobotips) cerr << "problem opening " << name << endl;
- string rname;
- while(Frobotips >> rname) {
- if(rname.at(0) == '#') {
- ignoreToEndOfLine(Frobotips);
- continue;
- }
- toupper(rname);
- string rip;
- Frobotips >> rip;
- string::size_type i = rip.find(':');
- try {
- int port = 0;
- if(i != string::npos) {
- port = atoi(string(rip, i+1, rip.size()-i-1).c_str());
- rip.resize(i);
- }
- if(rname == "OPTRADER") {
- ignoreToEndOfLine(Frobotips);
- cerr << "adding optrader@" << rip << ':' << port << endl;
- tclient->open(rip.c_str(), port);
- } else if(rname == "MAPSERVER") {
- ignoreToEndOfLine(Frobotips);
- cerr << "adding mapserver@" << rip << ':' << port << endl;
- if(port) mclient->open(rip.c_str(), port);
- else mclient->open(rip.c_str());
- } else {
- string voice, safeness;
- Frobotips >> voice >> safeness;
- ignoreToEndOfLine(Frobotips);
- cerr << "adding robot@" << rip << ':' << port << ' ' << voice << ' ' << safeness << endl;
- addRobot(rname, rip, voice, safeness, port);
- }
- } catch(RobotClientException e) {
- cerr << "unable to add robot " << rname << '(' << rip << ')' << endl;
- cerr << e.what() << endl;
- }
- }
-}
-
//---------------------------
// receiving messages
#ifdef WIN32
-void RobotClient::readMessages(void *thisp)
+void RobotsClient::readMessages(void *thisp)
#else
-void* RobotClient::readMessages(void *thisp)
+void* RobotsClient::readMessages(void *thisp)
#endif
{
- RobotClient* me = (RobotClient *) thisp;
- cerr << "About to start reading thread" << endl;
+ RobotsClient* me = (RobotsClient *) thisp;
+ debug("client") << "About to start reading thread" << endl;
for(;;) {
bool gotAMessage = false;
- for(map<string, Boeing::RobotClient*>::iterator i = me->robotAddress.begin();
- i != me->robotAddress.end(); i++) {
- const Boeing::MsgRobot* mesg = i->second->getNextMessage();
+ for(iterator i = me->begin(); i != me->end(); i++) {
+ const Boeing::MsgRobot* mesg = i->getNextMessage();
if(mesg == NULL) continue;
gotAMessage = true;
- Msg* m = Msg::interpret(mesg->buff, i->first);
+ Msg* m = Msg::interpretBoeingPacket(string(mesg->buff, sizeof(Boeing::MsgRobot)), i->getName());
if(m == NULL) continue;
- //if(!dynamic_cast<MsgRobLocationPP*>(m)) {
- //report unless you get a monotonous location message
- cerr << "robot " << i->first << ": " << *m << endl;
- //}
+ //report unless you get a monotonous location message
+ if(dynamic_cast<MsgRobLocation*>(m))
+ debug("client") << "robot " << i->getName() << ": " << m << endl;
+ else
+ info("client") << "robot " << i->getName() << ": " << m << endl;
{
//[2005-09-22] (tk): don't worry about pings for now
//MsgPingPP* ping = dynamic_cast<MsgPingPP*>(m);
@@ -245,108 +157,36 @@
// me->sendPacket(ping->handle(), MsgPingPP(me->handle_));
//}
}
- //cerr << "bout to call back" << endl;
- if (me->callback_ != NULL) {
- (*me->callback_)(m);
- } else {
- me->callbackClass_->callback(m);
- }
+ me->robotCallback_->processRobotMessage(m);
delete m;
}
if(!gotAMessage) Sleep(500);
}
}
-/*
-void RobotClient::broadcast() {
- //form broadcast addr
- sockaddr_in st_broadcast_addr;
- {
- memset(&st_broadcast_addr, 0, sizeof(struct sockaddr_in));
- st_broadcast_addr.sin_family = AF_INET;
- inet_pton(AF_INET, BROADCASTADDR, &st_broadcast_addr.sin_addr);
- st_broadcast_addr.sin_port = htons(PORT);
- }
+vector<string> RobotsClient::getClientList() const {
+ vector<string> ret;
+ for(const_iterator i = begin(); i != end(); i++) {
+ ret.push_back(i->getName());
+ }
+ return ret;
}
-*/
-vector<string> RobotClient::getClientList() const {
- vector<string> ret;
- for(map<string, Boeing::RobotClient*>::const_iterator i = robotAddress.begin();
- i != robotAddress.end();
- i++) {
- ret.push_back(i->first);
- }
- return ret;
+RobotClient* RobotsClient::find(const string& name) {
+ iterator i = set<RobotClient>::find(RobotClient(name));
+ if (i == end()) {
+ warn << "didn;t find robotclient for " << name << endl;
+ return NULL;
+ } else {
+ warn << "found robotclient " << ((void*)&*i) << " for " << name << endl;
+ }
+ return &*i;
}
-string RobotClient::find(const string& host, int port) const {
- for(map<string, Boeing::RobotClient*>::const_iterator i = robotAddress.begin();
- i != robotAddress.end();
- i++) {
- if(i->second->getHost() == host && (port == 0 || i->second->getPort() == port)) return i->first;
- }
- return string();
-}
-
-string RobotClient::robotVoice(const string& robot_name) const {
- map<string, string>::const_iterator i = robotVoice_.find(robot_name);
- if(i != robotVoice_.end()) return i->second;
- return string();
-}
-
-void RobotClient::wait() const {
+void RobotsClient::wait() const {
#ifdef WIN32
WaitForSingleObject(serverThread_, INFINITE);
#else
pthread_join(serverThread_, NULL);
#endif
}
-
- /*
- //here's some stuff that I might want to incorporate into udpsocket
- if (nready == SOCKET_ERROR) {
- select_error = WSAGetLastError();
- switch(select_error) {
- case WSANOTINITIALISED:
- throw RobotClientException("A successful WSAStartup call must occur before using this function.");
- case WSAEFAULT:
- throw RobotClientException("The name parameter is not a valid part of the user address space.");
- case WSAENETDOWN:
- throw RobotClientException("The network subsystem has failed.");
- case WSAEINVAL:
- throw RobotClientException("Invalid");
- case WSAEINTR:
- throw RobotClientException("A blocking Windows Socket 1.1 call was canceled through WSACancelBlockingCall.");
- case WSAEINPROGRESS:
- throw RobotClientException("A blocking Windows Sockets 1.1 call is in progress, or the service provider is still processing a callback function.");
- case WSAENOTSOCK:
- throw RobotClientException("Not a socket");
- default:
- throw RobotClientException("unknown select exception");
- }
- }
- }
- if (rv == SOCKET_ERROR) {
- socket_error = WSAGetLastError();
- switch (socket_error) {
- case WSANOTINITIALISED: throw RobotClientException("A successful WSAStartup call must occur before using this function.");
- case WSAENETDOWN: throw RobotClientException(" The network subsystem has failed.");
- case WSAEFAULT: throw RobotClientException(" The buf parameter is not completely contained in a valid part of the user address space.");
- case WSAENOTCONN: throw RobotClientException(" The socket is not connected.");
- case WSAEINTR: throw RobotClientException(" The (blocking) call was canceled through WSACancelBlockingCall.");
- case WSAEINPROGRESS: throw RobotClientException(" A blocking Windows Sockets 1.1 call is in progress, or the service provider is still processing a callback function.");
- case WSAENETRESET: throw RobotClientException(" The connection has been broken due to the keep-alive activity detecting a failure while the operation was in progress.");
- case WSAENOTSOCK: throw RobotClientException(" The descriptor is not a socket.");
- case WSAEOPNOTSUPP: throw RobotClientException(" MSG_OOB was specified, but the socket is not stream-style such as type SOCK_STREAM, OOB data is not supported in the communication domain associated with this socket, or the socket is unidirectional and supports only send operations.");
- case WSAESHUTDOWN: throw RobotClientException(" The socket has been shut down; it is not possible to receive on a socket after shutdown has been invoked with how set to SD_RECEIVE or SD_BOTH.");
- case WSAEWOULDBLOCK: throw RobotClientException(" The socket is marked as nonblocking and the receive operation would block.");
- case WSAEMSGSIZE: throw RobotClientException(" The message was too large to fit into the specified buffer and was truncated.");
- case WSAEINVAL: throw RobotClientException(" The socket has not been bound with bind, or an unknown flag was specified, or MSG_OOB was specified for a socket with SO_OOBINLINE enabled or (for byte stream sockets only) len was zero or negative.");
- case WSAECONNABORTED: throw RobotClientException(" The virtual circuit was terminated due to a time-out or other failure. The application should close the socket as it is no longer usable.");
- case WSAETIMEDOUT: throw RobotClientException(" The connection has been dropped because of a network failure or because the peer system failed to respond.");
- case WSAECONNRESET: throw RobotClientException(" The virtual circuit was reset by the remote side executing a hard or abortive close. The application should close the socket as it is no longer usable. On a UPD-datagram socket this error would indicate that a previous send operation resulted in an ICMP \"Port Unreachable\" message.");
- default: throw RobotClientException("unknown recv error");
- }
- }
- */
Modified: TeamTalk/Agents/PrimitiveComm/robot_client.hpp
===================================================================
--- TeamTalk/Agents/PrimitiveComm/robot_client.hpp 2006-11-09 20:10:38 UTC (rev 545)
+++ TeamTalk/Agents/PrimitiveComm/robot_client.hpp 2006-12-07 19:44:59 UTC (rev 546)
@@ -1,8 +1,6 @@
#ifndef __ROBOT_CLIENT_H__
#define __ROBOT_CLIENT_H__
-// client class to manage UDP connections to robots
-
#ifdef WIN32
#include <Winsock2.h>
#else
@@ -20,23 +18,13 @@
#include <map>
#include <vector>
#include <string>
-#include "robot_packet.hpp"
+#include "robot_packet2.h"
#include "udpsocket.h"
#include <boeing_trader_client.h>
#include <boeing_robot_client.h>
#include <boeing_map_client.h>
-#ifdef WIN32
-#define sleep(x) Sleep((x)*1000)
-#endif
-
-//[2005-09-29] (tk): hack - please repair
-//#define OPTRADER_IP "192.168.99.210"
-//#define OPTRADER_IP "128.2.179.53" (gs219)
-//#define OPTRADER_PORT 32790
-//#define OPTRADER_PORT 5001
-
using namespace std;
namespace TeamTalk {
@@ -49,41 +37,63 @@
const char* what() throw() {return error.c_str();}
};
-class CallBackClass {
+/**
+ * RobotCallback is an abstract interface class for the processMessage method.
+ * Whatever class you wish to use to process messages needs to inherit this
+ * class and implement processMessage.
+ */
+class RobotCallback {
public:
- virtual ~CallBackClass(){};
- virtual void callback(const Msg* msg)=0;
+ virtual void processRobotMessage(const Msg* m)=0;
};
-class RobotClient {
+/**
+ * The RobotClient class encapsulates everything we need to know to talk to a
+ * robot.
+ */
+class RobotClient : public Boeing::RobotClient {
private:
- static const unsigned short PORT;
- map<string, Boeing::RobotClient*> robotAddress; //for mapping names to robot clients
- map<string, string> robotVoice_; //for mapping robot names to voice names
- //map<Boeing::RobotClient, string> addressRobot; //for mapping robot clients to names
-
+ string name_; //the robot's name
+ string voice_; //the name of the robot's voice
+
+protected:
+ void connect(const string& loc, const string& voice, const string& safeness,
+ unsigned short port);
+
+public:
+ //spawnback is a hook for robot instantiation
+ //This is useful because the primitivecomm library handles robot instantiation
+ //and descruction, but may not know everything that needs to be done. By
+ //default it is set to do nothing.
+ static void (*spawnback)(const string& name, const string& safeness);
+ RobotClient(const string& name);
+ RobotClient(const string& name, const string& loc, const string& voice,
+ const string& safeness, unsigned short port=0);
+
+ string getName() const;
+ string getVoice() const;
+
+ bool operator<(const RobotClient& rhs) const;
+};
+
+class RobotsClient : public set<RobotClient> {
+private:
char hostname[Boeing::SADDR_LENGTH]; // our host name
char ip[16]; // our ip address in presentation format
string handle_; //our handle
#ifdef WIN32
static void readMessages(void *thisp);
- //static void keepalive(void *thisp);
HANDLE serverThread_;
#else
static void* readMessages(void *thisp);
- //static void* keepalive(void *thisp);
pthread_t serverThread_;
#endif
- void (*callback_)(const Msg*);
- CallBackClass *callbackClass_;
+ RobotCallback* robotCallback_;
- void (*spawnback_)(const string& name, const string& safeness);
-
protected:
void initializeSocketLayer();
- //void connectOpTrader();
void getMyAddress();
#ifdef WIN32
HANDLE spawnServer();
@@ -93,81 +103,15 @@
public:
// fill out any constructor/destructor info you need
- RobotClient(const string& handle, void (*c)(const Msg*), void (*n)(const string&, const string&)=NULL);
- RobotClient(const string& handle, CallBackClass* c);
- ~RobotClient();
+ RobotsClient(const string& handle, RobotCallback* robotCallback);
+ ~RobotsClient();
// returns true if robot wasn't already added
- bool addRobot(const string& name, const string& loc, const string& voice, const string& safeness, unsigned short port=0);
- bool addRobot(const string& name, in_addr addr);
- bool addRobot(const string& name, sockaddr_in addr);
- //adds undiscoverable robots from a file
- void addRobotsFile(const string& name,
- Boeing::TraderClient* tclient, Boeing::MapClient* mclient);
+ bool addRobot(const string& name, const string& loc, const string& voice,
+ const string& safeness, unsigned short port=0);
- // --------------------------
- // send commands
- /*
- template<class M> bool sendPacketToOptrader(const M& msg) const {
- cerr << "sending packet to optrader" << endl;
- M message(msg);
- Msg::stamp(message, hostname, "OpTrader");
- cerr << "to optrader: " << message << endl;
- cerr << "size is " << sizeof(*message.content()) << endl;
- cerr << hex;
- for(int i=0; i<sizeof(*message.content()); i++) {
- cerr << (int)(*(((const char*)message.content())+i));
- }
- cerr << dec << endl;
- int nbytes =
- opSocket->send((const char*)message.content(), sizeof(*message.content()));
- cerr << "sent " << nbytes << " bytes" << endl;
- return (nbytes == sizeof(*message.content()));
- }
- */
-
- Boeing::RobotClient* find(const string& s);
- string find(const string& host, int port=0) const; //get the name of a robot from the ip or port
-
- /*
- template<class M>
- bool sendPacket(const string& robot, const M& msg) const {
- M message(msg);
- Msg::stamp(message, hostname, robot);
-
- return sendBuf(robot, (const char*)message.content(), sizeof(*message.content()));
- }
- */
-
- /*
- bool sendBuf(const string& robot, const char* msg, int size) {
- map<string, Boeing::RobotClient>::iterator i = robotAddress.find(robot);
- if(i == robotAddress.end()) {
- cerr << "problem sending to unknown bot: " << robot << endl;
- return 0;
- }
- Boeing::MsgCmd msgcmd;
- memcpy(msgcmd.buff, msg, size);
- return (i->second.sendPacket((Boeing::MsgHeader*)&msgcmd))
- }
-*/
-
- // send to all robots
-/*
- template<class M> bool sendAll(const M& msg) const {
- bool retval = true;
- for(map<string, Boeing::RobotClient>::iterator i=robotAddress.begin();
- i!=robotAddress.end(); i++)
- retval &= sendPacket(i->first, msg);
- return retval;
- }
-*/
-
- void broadcast();
-
- //getters
vector<string> getClientList() const;
- string robotVoice(const string& robot_name) const;
+ RobotClient* find(const string& name);
//wait for thread
void wait() const;
Deleted: TeamTalk/Agents/PrimitiveComm/robot_packet.h
===================================================================
--- TeamTalk/Agents/PrimitiveComm/robot_packet.h 2006-11-09 20:10:38 UTC (rev 545)
+++ TeamTalk/Agents/PrimitiveComm/robot_packet.h 2006-12-07 19:44:59 UTC (rev 546)
@@ -1,203 +0,0 @@
-#ifndef __ROBOT_PACKET_H__
-#define __ROBOT_PACKET_H__
-
-#ifdef WIN32
-//msvc doesn't seem to have stdint.h
-typedef unsigned short uint16_t;
-typedef short int16_t;
-#else
-#include <stdint.h>
-#endif
-
-#include <time.h>
-
-// Packet structures for sending translated commands
-// between human operators and robots
-
-//----------------------------------------------------
-// header packet for all messages
-
-// number of bytes defining a senders hostname
-#define SADDR_LENGTH 32
-
-// len - length in bytes of whole packet (including header)
-// type - msg id
-// tstamp - time of transmission in secs since Jan 1, 1970
-// sender - senders hostname
-struct MsgHeader {
- uint16_t type;
- uint16_t len;
- double tstamp;
-};
-
-// task id types
-typedef uint16_t TaskID;
-
-//----------------------------------------------------
-// Message ID's
-
-// Task Requests
-static const uint16_t MsgIdCmdHalt = 0x0000;
-static const uint16_t MsgIdCmdGoTo = 0x0001;
-static const uint16_t MsgIdCmdHome = 0x0002;
-static const uint16_t MsgIdCmdPause = 0x0003;
-static const uint16_t MsgIdCmdResume = 0x0004;
-static const uint16_t MsgIdCmdFollow = 0x0005;
-static const uint16_t MsgIdCmdCover = 0x0006;
-static const uint16_t MsgIdCmdLoad = 0x0007;
-static const uint16_t MsgIdCmdUnload = 0x0008;
-
-static const uint16_t MsgIdCmdPlayAction = 0x0010;
-
-// Information Requests
-static const uint16_t MsgIdReqLocation = 0x2000;
-
-// Robot to Human status messages
-static const uint16_t MsgIdRobAck = 0x8000;
-static const uint16_t MsgIdRobDone = 0x4000;
-static const uint16_t MsgIdRobLocation = 0x4001;
-
-static const uint16_t MsgIdPlayActionAck = 0x0010;
-
-// Handshaking
-static const uint16_t MsgIdPing = 0x4002;
-
-//----------------------------------------------------
-// Task Requests
-
-struct MsgCmdTask : public MsgHeader {
- TaskID taskid;
-};
-
-// message packets
-// Halt message -> for stopping robots _now_
-typedef MsgCmdTask MsgCmdHalt;
-
-// Go to an x,y location in absolute/relative coord frame
-// default is in global frame
-// (x,y) - point in meters
-// angle - orientation in radians
-// attr - attributes, default - no angle, global frame
-struct MsgCmdGoTo : public MsgCmdTask {
- static const uint16_t UseAngle = 0x0100;
- static const uint16_t Relative = 0x0001;
-
- uint16_t attr;
- float x, y;
- float angle;
-
- bool useAngle() { return (attr & UseAngle); }
- bool useRelative() { return (attr & Relative); }
- bool useGlobal() { return (!useRelative()); }
-};
-
-
-typedef MsgCmdTask MsgCmdHome;
-typedef MsgCmdTask MsgCmdPause;
-typedef MsgCmdTask MsgCmdResume;
-
-struct MsgCmdFollow : public MsgCmdTask {
- char leader[SADDR_LENGTH];
-};
-
-struct RP_Polygon {
- static const int MAX_POLYGON=10;
-
- //up to 10-sided
- int n; //number of vertices
- float xpoint[MAX_POLYGON+1], ypoint[MAX_POLYGON+1];
-};
-
-struct MsgCmdCover : public MsgCmdTask {
- //only axis-aligned rectangles -a type of polygon- are currently supported
- RP_Polygon cover_shape;
-};
-
-typedef MsgCmdTask MsgCmdLoad;
-typedef MsgCmdTask MsgCmdUnload;
-
-// Request robot location in absolute absolute frame
-typedef MsgCmdTask MsgReqLocation;
-
-// play manager messages. Note these are variable length
-// action will be a null terminated string
-struct MsgCmdAction : public MsgCmdTask {
- char action[1];
-};
-
-//----------------------------------------------------
-// Robot to Human messages
-
-// acknowledgement messages
-struct MsgRobAck : public MsgHeader {
- TaskID taskid; // responding task id
-};
-
-// acknowledgement message for play manager
-struct MsgPlayActionAck : public MsgHeader {
- TaskID taskid;
- int16_t status;
-
- static const int16_t FAILED=-1;
- static const int16_t INPROGRESS=0;
- static const int16_t COMPLETED=1;
-
- int isCompleted() { return status==COMPLETED; }
- int isFailed() { return status<=FAILED; }
- int isInProgress() { return status==INPROGRESS; }
-};
-
-
-// robot done status message
-// status - task status [Succeeded,Failed]
-struct MsgRobDone : public MsgHeader {
- static const int16_t Succeeded = 0x0001;
- static const int16_t Failed = 0x0002;
-
- TaskID taskid; //task that has been completed
- int16_t status;
-
- bool isSuccess() { return (status == Succeeded); }
- bool isFailure() { return (!isSuccess()); }
-};
-
-// robot location status message
-// (x,y) - location in global frame in meters
-// angle - angle in global frame in radians
-// moving - flag to indicate robot is in motion
-struct MsgRobLocation : public MsgHeader {
- float x, y;
- float angle;
- int16_t moving;
- bool operator!=(const MsgRobLocation& rhs) const {
- return rhs.x != x ||
- rhs.y != y ||
- rhs.angle != angle ||
- rhs.moving != moving;
- };
-};
-
-// Handshaking
-struct MsgPing : public MsgHeader {
- char handle[0xff];
-};
-
-/*
-union MsgUnion {
- MsgCmdHalt halt;
- MsgCmdGoTo go;
- MsgCmdHome home;
- MsgCmdPause pause;
- MsgCmdResume resume;
- MsgCmdFollow follow;
- MsgCmdCover cover;
- MsgCmdLoad load;
- MsgCmdUnload unload;
- MsgReqLocation loc;
- MsgRobAck ack;
- MsgRobDone done;
- MsgRobLocation location;
-};
-*/
-
-#endif
Modified: TeamTalk/Agents/PrimitiveComm/robot_packet.hpp
===================================================================
--- TeamTalk/Agents/PrimitiveComm/robot_packet.hpp 2006-11-09 20:10:38 UTC (rev 545)
+++ TeamTalk/Agents/PrimitiveComm/robot_packet.hpp 2006-12-07 19:44:59 UTC (rev 546)
@@ -83,19 +83,21 @@
const Boeing::MsgCmdAction* content() const;
};
-class MsgCmdSetPos : public MsgCmdActionPP {
-private:
- double x_, y_, angle_;
+class MsgCmdSetPosPP : public Msg {
+ friend class Msg;
protected:
- static const char *const scan_template;
- bool parse();
+ Boeing::MsgCmdAction content_;
+ Boeing::MsgHeader* header();
public:
- MsgCmdSetPos(const Boeing::MsgCmdAction& c);
- MsgCmdSetPos(double x, double y, double a);
- double getX() const;
- double getY() const;
- double getAngle() const;
+ MsgCmdSetPosPP(const Boeing::MsgCmdAction& c);
+ MsgCmdSetPosPP(double x, double y, double a);
+ //double getX() const;
+ //double getY() const;
+ //double getAngle() const;
+ MsgCmdSetPosPP* clone() const;
+ const Boeing::MsgCmdAction* content() const;
string action_string() const;
+ string render() const;
};
class MsgCmdHaltPP : public Msg {
Added: TeamTalk/Agents/PrimitiveComm/robot_packet2.cpp
===================================================================
--- TeamTalk/Agents/PrimitiveComm/robot_packet2.cpp (rev 0)
+++ TeamTalk/Agents/PrimitiveComm/robot_packet2.cpp 2006-12-07 19:44:59 UTC (rev 546)
@@ -0,0 +1,592 @@
+#include "robot_packet2.h"
+#include "utils.h"
+#include "win_netutils.h"
+
+//these are for Msg::stamp
+#include <sys/types.h>
+#include <sys/timeb.h>
+
+#include <sstream>
+
+using namespace std;
+using namespace geometry;
+
+// MalformedPacketException *******************************************
+
+MalformedPacketException::MalformedPacketException(const string& context, const string& rep) throw() {
+ ostringstream e;
+ e << context << ": " << rep;
+ error = e.str();
+}
+
+MalformedPacketException::~MalformedPacketException() throw() {}
+
+const char* MalformedPacketException::what() throw() {return error.c_str();}
+
+// Msg ****************************************************************
+
+int Msg::taskIDCounter = 0;
+const int Msg::defaultPriority = 1;
+
+//normal instantiation
+Msg::Msg(string sender) : sender_(sender) {}
+//instantiation from Boeing packet
+Msg::Msg(string sender, double tstamp) : sender_(sender), tstamp_(tstamp) {}
+
+Msg::~Msg() {}
+
+Msg* Msg::interpretBoeingPacket(const string& m, const string& sender)
+{
+ const Boeing::MsgHeader *h =
+ reinterpret_cast<const Boeing::MsgHeader*>(m.c_str());
+ double tstamp = h->tstamp;
+ int type = h->type;
+ //messages to robots
+ if(type == Boeing::ROB_ACTION_ECHO || type == Boeing::CMD_ACTION) {
+ debug("packet") << "got: CMD_ACTION or ROB_ACTION_ECHO" << endl;
+ return MsgCmd::interpretBoeingPlayAction(
+ reinterpret_cast<const Boeing::MsgCmdAction*>(m.c_str()), sender, tstamp);
+ }
+ if(type == Boeing::CMD_COVER) {
+ warn("packet") << "unhandled type: CMD_COVER " << endl;
+ return NULL;
+ }
+ if(type == Boeing::CMD_FOLLOW) {
+ warn("packet") << "unhandled type: CMD_FOLLOW " << endl;
+ return NULL;
+ }
+ if(type == Boeing::CMD_GOTO) {
+ warn("packet") << "unhandled type: CMD_GOTO " << endl;
+ return NULL;
+ }
+ if(type == Boeing::CMD_HALT) {
+ warn("packet") << "unhandled type: CMD_HALT " << endl;
+ return NULL;
+ }
+ if(type == Boeing::CMD_HOME) {
+ warn("packet") << "unhandled type: CMD_HOME " << endl;
+ return NULL;
+ }
+ if(type == Boeing::CMD_PAUSE) {
+ warn("packet") << "unhandled type: CMD_PAUSE " << endl;
+ return NULL;
+ }
+ if(type == Boeing::CMD_RESUME) {
+ warn("packet") << "unhandled type: CMD_RESUME " << endl;
+ return NULL;
+ }
+ if(type == Boeing::CMD_SETPOS) {
+ warn("packet") << "unhandled type: CMD_SETPOS " << endl;
+ return NULL;
+ }
+ if(type == Boeing::REQ_LOCATION) {
+ debug("packet") << "got: REQ_LOCATION" << endl;
+ return new MsgReqLocation(sender, tstamp);
+ }
+ if(type == Boeing::REQ_IMAGE) {
+ debug("packet") << "got: REQ_IMAGE" << endl;
+ return new MsgMapReq(sender, tstamp,
+ reinterpret_cast<const Boeing::MsgMapReq*>(m.c_str())->invoice);
+ }
+ //messages from robots
+ if(type == Boeing::ROB_LOCATION) {
+ debug("packet") << "got ROB_LOCATION" << endl;
+ const Boeing::MsgRobLocation* brl =
+ reinterpret_cast<const Boeing::MsgRobLocation*>(m.c_str());
+ return new MsgRobLocation(sender, brl->tstamp,
+ geometry::Point<float>(brl->x, brl->y), brl->angle, (brl->moving != 0));
+ }
+ if(type == Boeing::ROB_ACK) {
+ debug("packet") << "got: ROB_ACK" << endl;
+ return new MsgRobAck(sender, tstamp,
+ reinterpret_cast<const Boeing::MsgRobAck*>(m.c_str())->taskid);
+ }
+// case Boeing::ROB_ACTION_ACK:
+// warn("packet") << "unhandled type: ROB_ACTION_ACK " << h->type << endl;
+// break;
+ if(type == Boeing::ROB_DONE) {
+ debug("packet") << "got: ROB_DONE" << endl;
+ const Boeing::MsgRobDone* bp = reinterpret_cast<const Boeing::MsgRobDone*>(m.c_str());
+ return new MsgRobDone(sender, tstamp,
+ bp->taskid, (bp->status == Boeing::SUCCEEDED));
+ }
+ if(type == Boeing::ROB_PLAY_HALT) {
+ warn("packet") << "unhandled type: ROB_PLAY_HALT " << endl;
+ return NULL;
+ }
+ if(type == Boeing::ROB_IMAGE) {
+ debug("packet") << "got: ROB_IMAGE" << endl;
+ const Boeing::MsgMap* bp = reinterpret_cast<const Boeing::MsgMap*>(m.c_str());
+ return new MsgMap(sender, tstamp, bp->invoice, bp->seq, bp->x, bp->y,
+ string((const char *)bp->map, bp->array_length), MsgMap::JPEG);
+ }
+ //messages to trader
+// case Boeing::TASK_CANCEL:
+// warn("packet") << "unhandled type: TASK_CANCEL " << endl;
+// break;
+ //messages from trader
+ if(type == Boeing::INFO) {
+ warn("packet") << "unhandled type: INFO " << endl;
+ return NULL;
+ }
+// case Boeing::TASK_ACK:
+// warn("packet") << "unhandled type: TASK_ACK " << endl;
+// break;
+// case Boeing::TASK_DONE:
+// warn("packet") << "unhandled type: TASK_DONE " << endl;
+// break;
+ //messages to mapserver/imageserver
+// case Boeing::MAP_ACK:
+// warn("packet") << "unhandled type: MAP_ACK " << endl;
+// break;
+// case Boeing::MAP_KEEPALIVE:
+// warn("packet") << "unhandled type: MAP_KEEPALIVE " << endl;
+// break;
+ //messages from mapserver/imageserver
+// case Boeing::MAP_DIFF:
+// warn("packet") << "unhandled type: MAP_DIFF " << endl;
+// break;
+// case Boeing::MAP_FULL:
+// warn("packet") << "unhandled type: MAP_FULL " << endl;
+// break;
+ warn("packet") << "unknown type: " << h->type << endl;
+ return NULL;
+}
+
+void Msg::stamp(Boeing::MsgHeader& h)
+{
+#ifdef WIN32
+ struct __timeb64 timebuffer;
+ _ftime64(&timebuffer);
+#else
+ struct timeb timebuffer;
+ ftime(&timebuffer);
+#endif
+ h.tstamp = timebuffer.time + (float)timebuffer.millitm/1000;
+}
+
+string Msg::getSender() const {return sender_;}
+
+// MsgCmd *************************************************************
+
+//normal instantiation
+MsgCmd::MsgCmd() : Msg(), taskID_(taskIDCounter++), priority_(defaultPriority) {}
+MsgCmd::MsgCmd(int priority, string sender)
+: Msg(sender), taskID_(taskIDCounter++), priority_(priority) {}
+//instantiation from Boeing packet
+MsgCmd::MsgCmd(string sender, double tstamp, int priority, int taskID)
+: Msg(sender, tstamp) {}
+
+int MsgCmd::getTaskID() const {return taskID_;}
+int MsgCmd::getPriority() const {return priority_;}
+
+MsgCmd* MsgCmd::interpretBoeingPlayAction(const Boeing::MsgCmdAction* m,
+ const string& sender, double tstamp)
+{
+ istringstream iaction(m->action);
+ string action_head;
+ iaction >> action_head;
+ if(action_head == "halt")
+ return new MsgCmdHalt(sender, tstamp, m->priority, m->taskid, m->action);
+ if(action_head == "goto" || action_head == "home")
+ return new MsgCmdGoTo(sender, tstamp, m->priority, m->taskid, m->action);
+ if(action_head == "pause")
+ return new MsgCmdPause(sender, tstamp, m->priority, m->taskid, m->action);
+ if(action_head == "resume")
+ return new MsgCmdPause(sender, tstamp, m->priority, m->taskid, m->action);
+ if(action_head == "follow")
+ return new MsgCmdFollow(sender, tstamp, m->priority, m->taskid, m->action);
+ if(action_head == "cover")
+ return new MsgCmdCover(sender, tstamp, m->priority, m->taskid, m->action);
+ if(action_head == "setpos")
+ return new MsgCmdSetPos(sender, tstamp, m->priority, m->taskid, m->action);
+ warn("packet") << "unknown action head: " << action_head << endl;
+ return NULL;
+}
+
+string MsgCmd::renderBoeingPacket() const
+{
+ string retval;
+ Boeing::MsgCmdAction bPacket =
+ Boeing::MsgCmdAction::factory(render().c_str(), priority_, taskID_);
+ retval.assign(reinterpret_cast<const char*>(&bPacket), bPacket.len);
+ return retval;
+}
+
+string MsgCmd::render() const
+{
+ ostringstream out;
+ out << "MsgCmd: ";
+ out << "sender " << sender_;
+ out << " priority " << priority_;
+ out << " tstamp " << tstamp_ << ' ';
+ out << renderBoeingPlayAction();
+ return out.str();
+}
+
+// MsgCmdHalt *********************************************************
+
+//normal instantiation
+MsgCmdHalt::MsgCmdHalt(): MsgCmd() {}
+MsgCmdHalt::MsgCmdHalt(int priority): MsgCmd(priority) {}
+
+//instantiation from Boeing packet
+MsgCmdHalt::MsgCmdHalt(string sender, double tstamp, int priority, int taskID,
+ string action)
+: MsgCmd(sender, tstamp, priority, taskID) {}
+
+string MsgCmdHalt::renderBoeingPlayAction() const {return "halt";}
+
+// MsgCmdGoTo *********************************************************
+
+//normal instantiation
+//turn radians
+MsgCmdGoTo::MsgCmdGoTo(float rad)
+: MsgCmd(), angle_(rad), useAngle_(true), absolute_(false) {}
+//move along a relative vector
+MsgCmdGoTo::MsgCmdGoTo(Point<float> loc, bool absolute)
+: MsgCmd(), loc_(loc), angle_(0), useAngle_(false), absolute_(absolute) {}
+MsgCmdGoTo::MsgCmdGoTo(Point<float> loc, float angle, bool useAngle, bool absolute, int priority, string sender)
+: MsgCmd(priority, sender), loc_(loc), angle_(angle), useAngle_(useAngle), absolute_(absolute) {}
+
+//instantiation from a Boeing packet
+MsgCmdGoTo::MsgCmdGoTo(string sender, double tstamp, int priority, int taskID, string action)
+: MsgCmd(sender, tstamp, priority, taskID) {
+ MalformedPacketException e("MsgCmdGoTo", action);
+ istringstream in(action);
+ string token;
+ if((in >> token).fail() || token != "goto") throw e;
+ if((in >> token).fail()) throw e;
+ if(token == "abs") absolute_ = true;
+ else if(token == "rel") absolute_ = false;
+ else throw e;
+ if((in >> loc_).fail()) throw e;
+ useAngle_ = !(in >> angle_).fail();
+}
+
+string MsgCmdGoTo::renderBoeingPlayAction() const {
+ ostringstream out;
+ out << "goto ";
+ if(absolute_) out << "abs ";
+ else out << "rel ";
+ out << loc_;
+ if(useAngle_) out << ' ' << angle_;
+ return out.str();
+}
+
+float MsgCmdGoTo::getX() const {return loc_.x;}
+float MsgCmdGoTo::getY() const {return loc_.y;}
+float MsgCmdGoTo::getAngle() const {return angle_;}
+bool MsgCmdGoTo::useAngle() const {return useAngle_;}
+bool MsgCmdGoTo::isAbsolute() const {return absolute_;}
+bool MsgCmdGoTo::isRelative() const {return !absolute_;}
+
+// MsgCmdHome *********************************************************
+
+//normal instantiation
+MsgCmdHome::MsgCmdHome() : MsgCmd() {}
+MsgCmdHome::MsgCmdHome(int priority, string sender) : MsgCmd(priority, sender) {}
+
+//instantiation from a Boeing packet
+MsgCmdHome::MsgCmdHome(string sender, double tstamp, int priority, int taskID, string action)
+: MsgCmd(sender, tstamp, priority, taskID) {}
+
+string MsgCmdHome::renderBoeingPlayAction() const {return "home";}
+
+// MsgCmdPause ********************************************************
+
+//normal instantiation
+MsgCmdPause::MsgCmdPause(int priority, string sender) : MsgCmd(priority, sender) {}
+
+//instantiation from a Boeing packet
+MsgCmdPause::MsgCmdPause(string sender, double tstamp, int priority, int taskID, string action)
+: MsgCmd(sender, tstamp, priority, taskID) {}
+
+string MsgCmdPause::renderBoeingPlayAction() const {return "pause";}
+
+// MsgCmdResume *******************************************************
+
+//normal instantiation
+MsgCmdResume::MsgCmdResume(int priority, string sender) : MsgCmd(priority, sender) {}
+
+//instantiation from a Boeing packet
+MsgCmdResume::MsgCmdResume(string sender, double tstamp, int priority, int taskID, string action)
+: MsgCmd(sender, tstamp, priority, taskID) {}
+
+string MsgCmdResume::renderBoeingPlayAction() const {return "resum,e";}
+
+// MsgCmdFollow *******************************************************
+
+//normal instantiation
+MsgCmdFollow::MsgCmdFollow() : MsgCmd() {}
+MsgCmdFollow::MsgCmdFollow(int priority, string sender) : MsgCmd(priority, sender) {}
+
+//instantiation from a Boeing packet
+MsgCmdFollow::MsgCmdFollow(string sender, double tstamp, int priority, int taskID, string action)
+: MsgCmd(sender, tstamp, priority, taskID) {}
+
+string MsgCmdFollow::renderBoeingPlayAction() const {return "follow";}
+
+// MsgCmdCover ********************************************************
+
+//normal instantiations
+MsgCmdCover::MsgCmdCover(const Point<float>& lower_left, const Point<float>& upper_right, int priority, string sender)
+: MsgCmd(priority, sender), polygon_(4) {
+ polygon_[0] = lower_left;
+ polygon_[1] = Point<float>(lower_left.x, upper_right.y);
+ polygon_[2] = upper_right;
+ polygon_[3] = Point<float>(upper_right.x, lower_left.y);
+}
+
+MsgCmdCover::MsgCmdCover(float x1, float x2, float y1, float y2, int priority, string sender)
+: MsgCmd(priority, sender), polygon_(4) {
+ polygon_[0] = Point<float>(x1, y1);
+ polygon_[1] = Point<float>(x1, y2);
+ polygon_[2] = Point<float>(x2, y2);
+ polygon_[3] = Point<float>(x2, y1);
+}
+
+MsgCmdCover::MsgCmdCover(const geometry::Polygon<float>& polygon, int priority, string sender)
+: MsgCmd(priority, sender), polygon_(polygon) {}
+
+//instantiation from a Boeing packet
+MsgCmdCover::MsgCmdCover(string sender, double tstamp, int priority, int taskID, string action)
+: MsgCmd(sender, tstamp, priority, taskID) {
+ MalformedPacketException e("MsgCmdCover", action);
+ istringstream in(action);
+ string token;
+ if((in >> token).fail() || token != "cover") throw e;
+ Point<float> p;
+ if((in >> p).fail()) throw e; //there should be at least one cover point
+ do {
+ polygon_.push_back(p);
+ in >> p;
+ } while(!in.fail());
+}
+
+geometry::Polygon<float> MsgCmdCover::getPolygon() const {return polygon_;}
+
+string MsgCmdCover::renderBoeingPlayAction() const {
+ ostringstream out;
+ out << "cover ";
+ for(geometry::Polygon<float>::const_iterator i = polygon_.begin(); i != polygon_.end(); i++) {
+ if(i != polygon_.begin()) out << ' ';
+ out << *i;
+ }
+ return out.str();
+}
+
+// MsgCmdSetPos *******************************************************
+
+//normal instantiation
+MsgCmdSetPos::MsgCmdSetPos(const Point<float>& loc, float angle, int priority, string sender)
+: MsgCmd(priority, sender), loc_(loc), angle_(angle) {}
+MsgCmdSetPos::MsgCmdSetPos(float x, float y, float angle, int priority, string sender)
+: MsgCmd(priority, sender), loc_(x, y), angle_(angle) {}
+
+//instantiation from a Boeing Packet
+MsgCmdSetPos::MsgCmdSetPos(string sender, double tstamp, int priority, int taskID, string action)
+: MsgCmd(sender, tstamp, priority, taskID) {
+ MalformedPacketException e("MsgCmdSetPos", action);
+ istringstream in(action);
+ string token;
+ if((in >> token).fail() || token != "setpos") throw e;
+ if((in >> loc_).fail()) throw e;
+ if((in >> angle_).fail()) throw e;
+}
+
+Point<float> MsgCmdSetPos::getLocation() const {return loc_;}
+float MsgCmdSetPos::getX() const {return loc_.x;}
+float MsgCmdSetPos::getY() const {return loc_.y;}
+float MsgCmdSetPos::getAngle() const {return angle_;}
+
+string MsgCmdSetPos::renderBoeingPlayAction() const {
+ ostringstream out;
+ out << "setpos " << loc_ << ' ' << angle_;
+ return out.str();
+}
+
+// MsgReqLocation *****************************************************
+
+//normal instantiation
+MsgReqLocation::MsgReqLocation(string sender) : Msg(sender) {}
+
+//instatiation from a Boeing packet
+MsgReqLocation::MsgReqLocation(string sender, double tstamp) : Msg(sender, tstamp) {}
+
+string MsgReqLocation::render() const {return "reqlocation";}
+
+string MsgReqLocation::renderBoeingPacket() const {
+ Boeing::MsgReqLocation packet =
+ Boeing::MsgReqLocation::factory(1, taskIDCounter++);
+ return string(reinterpret_cast<char *>(&packet), packet.len);
+}
+
+// MsgRobAck **********************************************************
+
+//normal instantiation
+MsgRobAck::MsgRobAck(int taskID, string sender) : Msg(sender), taskID_(taskID) {}
+
+//instatiation from a Boeing packet
+MsgRobAck::MsgRobAck(string sender, double tstamp, int taskID)
+: Msg(sender, tstamp), taskID_(taskID) {}
+
+string MsgRobAck::render() const {return "roback";}
+
+string MsgRobAck::renderBoeingPacket() const {
+ Boeing::MsgRobAck packet = Boeing::MsgRobAck::factory(taskID_);
+ return string(reinterpret_cast<char *>(&packet), packet.len);
+}
+
+// MsgRobDone *********************************************************
+
+//normal instantiation
+MsgRobDone::MsgRobDone(int taskID, bool success, string sender)
+: Msg(sender), taskID_(taskID), success_(success) {}
+
+//instantiation from a Boeing packet
+MsgRobDone::MsgRobDone(string sender, double tstamp, int taskID, bool success)
+: Msg(sender, tstamp), taskID_(taskID), success_(success) {}
+
+int MsgRobDone::getTaskID() const {return taskID_;}
+
+bool MsgRobDone::isSuccess() const {return success_;}
+
+string MsgRobDone::render() const {
+ ostringstream out;
+ out << "robdone: " << taskID_ << (success_? "SUCCEEDED": "FAILED");
+ return out.str();
+}
+
+string MsgRobDone::renderBoeingPacket() const {
+ Boeing::MsgRobDone packet = Boeing::MsgRobDone::factory(taskID_, success_);
+ return string(reinterpret_cast<char *>(&packet), packet.len);
+}
+
+// MsgRobLocation *****************************************************
+
+const float MsgRobLocation::tolerance = 0.05F; //5 cm tolerance to change-in position
+const float MsgRobLocation::angularTolerance = 0.03F; // ~2 deg tolerance
+
+//normal instantiation
+MsgRobLocation::MsgRobLocation()
+: Msg(), angle_(0), moving_(false) {}
+MsgRobLocation::MsgRobLocation(Point<float> loc, float angle, bool moving, string sender)
+: Msg(sender), loc_(loc), angle_(angle), moving_(moving) {}
+
+MsgRobLocation::MsgRobLocation(string sender, double tstamp, Point<float> loc, float angle, bool moving)
+: Msg(sender, tstamp), loc_(loc), angle_(angle), moving_(moving) {}
+
+Point<float> MsgRobLocation::getLocation() const {return loc_;}
+float MsgRobLocation::getX() const {return loc_.x;}
+float MsgRobLocation::getY() const {return loc_.y;}
+float MsgRobLocation::getAngle() const {return angle_;}
+bool MsgRobLocation::isMoving() const {return moving_;}
+
+bool MsgRobLocation::operator==(const MsgRobLocation& x) const {
+ return (loc_-x.loc_).length() <= tolerance &&
+ abs(angle_ - x.angle_) <= angularTolerance;
+}
+
+bool MsgRobLocation::operator!=(const MsgRobLocation& x) const {
+ return (loc_-x.loc_).length() > tolerance ||
+ abs(angle_ - x.angle_) <= angularTolerance;
+}
+
+string MsgRobLocation::render() const {
+ ostringstream out;
+ out << "roblocation: " << loc_ << angle_ << "rad "
+ << (moving_? "MOVING": "STILL");
+ return out.str();
+}
+
+string MsgRobLocation::renderBoeingPacket() const {
+ Boeing::MsgRobLocation packet =
+ Boeing::MsgRobLocation::factory(loc_.x, loc_.y, angle_, moving_);
+ return string(reinterpret_cast<char*>(&packet), packet.len);
+}
+
+// MsgMapReq **********************************************************
+
+int MsgMapReq::invoiceCounter_ = 0;
+
+//normal instantiation
+MsgMapReq::MsgMapReq(string sender) : Msg(sender), invoice_(invoiceCounter_++) {}
+
+//instantiation from a Boeing packet
+MsgMapReq::MsgMapReq(string sender, double tstamp, int invoice)
+: Msg(sender, tstamp), invoice_(invoice) {}
+
+int MsgMapReq::getInvoice() const {return invoice_;}
+
+string MsgMapReq::render() const {
+ ostringstream out;
+ out << "mapreq: " << invoice_;
+ return out.str();
+}
+
+string MsgMapReq::renderBoeingPacket() const {
+ Boeing::MsgMapReq packet = Boeing::MsgMapReq::factory();
+ return string(reinterpret_cast<char *>(&packet), packet.len);
+}
+
+// MsgMap *************************************************************
+
+//normal instantiation
+MsgMap::MsgMap(short invoice, int sequence, int width, int height, const string& imageData, MsgMap::Encoding encoding, string sender)
@@ Diff output truncated at 60000 characters. @@
More information about the TeamTalk-developers
mailing list