Revision: 7421
http://playerstage.svn.sourceforge.net/playerstage/?rev=7421&view=rev
Author: gerkey
Date: 2009-03-10 03:14:32 +0000 (Tue, 10 Mar 2009)
Log Message:
-----------
building and happy
Modified Paths:
--------------
code/websim/src/websim.cc
code/websim/src/websim.h
Modified: code/websim/src/websim.cc
===================================================================
--- code/websim/src/websim.cc 2009-03-10 03:07:02 UTC (rev 7420)
+++ code/websim/src/websim.cc 2009-03-10 03:14:32 UTC (rev 7421)
@@ -27,9 +27,13 @@
#include "websim.h"
+#include <boost/lexical_cast.hpp>
+
#include <assert.h>
#include <stdlib.h>
+using namespace websim;
+
WebSim::WebSim(const std::string& _fedfile,
const std::string& _host,
int _port) :
@@ -107,3 +111,185 @@
}
return false;
}
+
+bool
+WebSim::HandleURI(const std::string& model,
+ const std::string& prop,
+ const std::string& action,
+ struct evkeyvalq* kv,
+ std::string& response)
+{
+ // The special simulation model
+ if(model == "sim")
+ {
+ return HandleSimRequest(prop, action, kv, response);
+ }
+ // Everything else must be an existing model
+ else
+ {
+ return HandleModelRequest(model, prop, action, kv, response);
+ }
+}
+
+bool
+WebSim::HandleSimRequest(const std::string& prop,
+ const std::string& action,
+ struct evkeyvalq* kv,
+ std::string& response)
+{
+ // The special factory property
+ if(prop == "factory")
+ {
+ if(action == "create")
+ {
+ std::string name, type;
+ if(!GetValue(name, kv, "name") ||
+ !GetValue(type, kv, "type"))
+ {
+ response = "ERROR: Missing name and/or type argument for
sim/factor/create";
+ return false;
+ }
+
+ return(CreateModel(name, type, response));
+ }
+ else if(action == "create")
+ {
+ std::string name, type;
+ if(!GetValue(name, kv, "name"))
+ {
+ response = "ERROR: Missing name argument for sim/factor/create";
+ return false;
+ }
+
+ return(DeleteModel(name, response));
+ }
+ else
+ {
+ response = "ERROR: Unknown action " + action + " for sim/factory";
+ return false;
+ }
+ }
+ else
+ {
+ response = "ERROR: Unknown property " + prop + " for sim";
+ return false;
+ }
+}
+
+bool
+WebSim::HandleModelRequest(const std::string& model,
+ const std::string& prop,
+ const std::string& action,
+ struct evkeyvalq* kv,
+ std::string& response)
+{
+ if(action == "get")
+ {
+ if(prop == "pose")
+ {
+ Pose p;
+ Velocity v;
+ Acceleration a;
+ if(GetModelState(model, p, v, a, response))
+ {
+ char buf[1024];
+ snprintf(buf, sizeof(buf),
+ "%s's state: \n pose: (%.3f,%.3f,%.3f) (%.3f,%.3f,%.3f)\n"
+ " vel : (%.3f,%.3f,%.3f) (%.3f,%.3f,%.3f)\n"
+ " acc : (%.3f,%.3f,%.3f) (%.3f,%.3f,%.3f)\n",
+ model.c_str(),
+ p.x, p.y, p.z, p.r, p.p, p.a,
+ v.x, v.y, v.z, v.r, v.p, v.a,
+ a.x, a.y, a.z, a.r, a.p, a.a);
+ response = buf;
+ return true;
+ }
+ else
+ {
+ response = "ERROR: Failed to get pose for model " + model;
+ return false;
+ }
+ }
+ else
+ {
+ response = "ERROR: Unknown property " + prop + " for model " + model;
+ return false;
+ }
+ }
+ else if(action == "set")
+ {
+ if(prop == "pose")
+ {
+ std::string sx, sy, sz, sroll, spitch, syaw;
+
+ // Get pose first, fill in what the caller provided
+ Pose p;
+ Velocity v;
+ Acceleration a;
+ if(!GetModelState(model, p, v, a, response))
+ {
+ response = "Failed to get pose before setting it";
+ return false;
+ }
+ try
+ {
+ if(GetValue(sx, kv, "x"))
+ p.x = boost::lexical_cast<float>(sx);
+ if(GetValue(sy, kv, "y"))
+ p.y = boost::lexical_cast<float>(sy);
+ if(GetValue(sz, kv, "z"))
+ p.z = boost::lexical_cast<float>(sz);
+ if(GetValue(sroll, kv, "r"))
+ p.r = boost::lexical_cast<float>(sroll);
+ if(GetValue(spitch, kv, "p"))
+ p.p = boost::lexical_cast<float>(spitch);
+ if(GetValue(syaw, kv, "a"))
+ p.a = boost::lexical_cast<float>(syaw);
+
+ if(GetValue(sx, kv, "vx"))
+ v.x = boost::lexical_cast<float>(sx);
+ if(GetValue(sy, kv, "vy"))
+ v.y = boost::lexical_cast<float>(sy);
+ if(GetValue(sz, kv, "vz"))
+ v.z = boost::lexical_cast<float>(sz);
+ if(GetValue(sroll, kv, "vr"))
+ v.r = boost::lexical_cast<float>(sroll);
+ if(GetValue(spitch, kv, "vp"))
+ v.p = boost::lexical_cast<float>(spitch);
+ if(GetValue(syaw, kv, "va"))
+ v.a = boost::lexical_cast<float>(syaw);
+
+ if(GetValue(sx, kv, "ax"))
+ a.x = boost::lexical_cast<float>(sx);
+ if(GetValue(sy, kv, "ay"))
+ a.y = boost::lexical_cast<float>(sy);
+ if(GetValue(sz, kv, "az"))
+ a.z = boost::lexical_cast<float>(sz);
+ if(GetValue(sroll, kv, "ar"))
+ a.r = boost::lexical_cast<float>(sroll);
+ if(GetValue(spitch, kv, "ap"))
+ a.p = boost::lexical_cast<float>(spitch);
+ if(GetValue(syaw, kv, "aa"))
+ a.a = boost::lexical_cast<float>(syaw);
+ }
+ catch(boost::bad_lexical_cast e)
+ {
+ response = std::string("Failed to parse input value(s): ") +
+ e.what();
+ return false;
+ }
+
+ return(SetModelState(model, p, v, a, response));
+ }
+ else
+ {
+ response = "ERROR: Unknown property " + prop + " for model " + model;
+ return false;
+ }
+ }
+ else
+ {
+ response = "ERROR: Unknown action " + action;
+ return false;
+ }
+}
Modified: code/websim/src/websim.h
===================================================================
--- code/websim/src/websim.h 2009-03-10 03:07:02 UTC (rev 7420)
+++ code/websim/src/websim.h 2009-03-10 03:14:32 UTC (rev 7421)
@@ -37,16 +37,40 @@
#include <event.h>
#include <evhttp.h>
+namespace websim
+{
+
+class Pose;
+class Velocity;
+class Acceleration;
+
class WebSim
{
public:
WebSim(const std::string& _fedfile,
const std::string& _host,
- int _port); // TODO: 3 callbacks
+ int _port);
~WebSim();
void Update();
+ // Interface to be implemented by simulators
+ virtual bool CreateModel(const std::string& name,
+ const std::string& type,
+ std::string& error) = 0;
+ virtual bool DeleteModel(const std::string& name,
+ std::string& error) = 0;
+ virtual bool SetModelState(const std::string& name,
+ const Pose& p,
+ const Velocity& v,
+ const Acceleration& a,
+ std::string& error) = 0;
+ virtual bool GetModelState(const std::string& name,
+ Pose& p,
+ Velocity& v,
+ Acceleration& a,
+ std::string& error) = 0;
+
private:
std::string fedfile;
std::string host;
@@ -84,3 +108,50 @@
std::string& response);
void DeleteKeyVal(struct evkeyvalq* query_args);
};
+
+class Pose
+{
+public:
+ double x,y,z,r,p,a;
+
+ Pose() :
+ x(0), y(0), z(0), r(0), p(0), a(0)
+ {
+ // nothing to do
+ }
+
+ Pose( double x, double y, double z,
+ double r, double p, double a ) :
+ x(x), y(y), z(z), r(r), p(p), a(a)
+ {
+ // nothing to do
+ }
+
+};
+
+class Velocity : public Pose
+{
+public:
+ Velocity() : Pose()
+ {}
+
+ Velocity( double x, double y, double z,
+ double r, double p, double a ) :
+ Pose( x, y, z, r, p, a )
+ {}
+};
+
+class Acceleration : public Pose
+{
+public:
+ Acceleration() : Pose()
+ {}
+
+ Acceleration( double x, double y, double z,
+ double r, double p, double a ) :
+ Pose( x, y, z, r, p, a )
+ {}
+};
+
+
+}
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
