Revision: 7427
http://playerstage.svn.sourceforge.net/playerstage/?rev=7427&view=rev
Author: gerkey
Date: 2009-03-10 05:48:01 +0000 (Tue, 10 Mar 2009)
Log Message:
-----------
removed svn:externals
Modified Paths:
--------------
code/branches/federation/gazebo/webgazebo/SConscript
code/branches/federation/gazebo/webgazebo/WebGazebo.cc
code/branches/federation/gazebo/webgazebo/WebGazebo.hh
code/branches/federation/gazebo/webgazebo/main.cc
Property Changed:
----------------
code/branches/federation/gazebo/
Property changes on: code/branches/federation/gazebo
___________________________________________________________________
Modified: svn:externals
- websim
https://playerstage.svn.sourceforge.net/svnroot/playerstage/code/websim
+
Modified: code/branches/federation/gazebo/webgazebo/SConscript
===================================================================
--- code/branches/federation/gazebo/webgazebo/SConscript 2009-03-10
05:43:37 UTC (rev 7426)
+++ code/branches/federation/gazebo/webgazebo/SConscript 2009-03-10
05:48:01 UTC (rev 7427)
@@ -1,12 +1,6 @@
#Import variable
Import('*')
-# Build websim; it uses CMake
-import os
-print 'Building websim...'
-os.system('mkdir -p ../websim/build && cd ../websim/build && cmake .. && make')
-print '...done building websim.'
-
env.Append(CPPPATH = '#webgazebo')
libEnv = Environment (
@@ -22,9 +16,9 @@
#SHLINKCOMSTR = 'Linking $TARGET',
#LINKCOMSTR = 'Linking $TARGET',
- LIBS=Split('event gazebo'),
- LIBPATH=Split('#libgazebo'),
- CPPPATH=Split('#.')
+ LIBS=Split('event gazebo websim'),
+ LIBPATH=Split('#libgazebo ' + install_prefix + '/lib'),
+ CPPPATH=Split('#. ' + install_prefix + '/include')
)
# We need Gazebo's quaternion functionality
@@ -49,9 +43,9 @@
#SHLINKCOMSTR = 'Linking $TARGET',
#LINKCOMSTR = 'Linking $TARGET',
- LIBS=Split('webgazebo event gazebo'),
- LIBPATH=Split('#libgazebo ' + os.getcwd()),
- CPPPATH=Split('#.')
+ LIBS=Split('websim webgazebo event gazebo'),
+ LIBPATH=Split('#libgazebo ' + os.getcwd() + ' ' + install_prefix + '/lib'),
+ CPPPATH=Split('#. ' + install_prefix + '/include')
)
webgazebo = exeEnv.Program('webgazebo', 'main.cc')
Modified: code/branches/federation/gazebo/webgazebo/WebGazebo.cc
===================================================================
--- code/branches/federation/gazebo/webgazebo/WebGazebo.cc 2009-03-10
05:43:37 UTC (rev 7426)
+++ code/branches/federation/gazebo/webgazebo/WebGazebo.cc 2009-03-10
05:48:01 UTC (rev 7427)
@@ -35,8 +35,6 @@
#include <time.h>
#include <string.h>
-#include <boost/lexical_cast.hpp>
-
#include "../server/Quatern.hh"
// TODO:
@@ -45,21 +43,12 @@
// - expose URI command to run for 1 tick
// - do performance test
-WebGazebo::WebGazebo(std::string _host, int _port, double dtol, double atol) :
- host(_host), port(_port), sq_dist_tol(dtol*dtol), sq_ang_tol(atol*atol)
+WebGazebo::WebGazebo(const std::string& fedfile,
+ const std::string& host, int port,
+ double dtol, double atol) :
+ websim::WebSim(fedfile, host, port),
+ sq_dist_tol(dtol*dtol), sq_ang_tol(atol*atol)
{
- // Set up the HTTP server
- // Not sure whether it's safe to do this more that once in one process
- event_init();
-
- printf("[webgazebo] Starting HTTP server listening on %s:%d...",
- this->host.c_str(), this->port);
- fflush(stdout);
- this->eh = evhttp_start(this->host.c_str(), this->port);
- assert(eh);
- evhttp_set_gencb(eh, &WebGazebo::EventCallback, (void*)this);
- puts("Done.");
-
// Hook up to Gazebo
printf("[webgazebo] Opening Gazebo simulation interface...");
fflush(stdout);
@@ -70,9 +59,6 @@
// Open the Simulation Interface; let exceptions leak out
this->simIface->Open(this->client, "default");
this->factoryIface->Open(this->client, "factory_iface");
- // Get the list of models
- if(!GetModelList())
- throw("Failed to get list of models from Gazebo");
puts("Done.");
puts("[webgazebo] Ready");
@@ -82,34 +68,20 @@
{
delete this->simIface;
delete this->client;
-
- // No event_fini() to call...
}
bool
-WebGazebo::GetModelList()
+WebGazebo::GetModelPVA(const std::string& name,
+ websim::Pose& p,
+ websim::Velocity& v,
+ websim::Acceleration& a,
+ std::string& error)
{
- // TODO
- this->models["pioneer2dx_model1"] = 0;
- return true;
-}
-
-bool WebGazebo::HasModel(const std::string& name)
-{
- // TODO
- //std::map<std::string,int>::const_iterator it = models.find(name);
- //return it != models.end();
- return true;
-}
-
-bool
-WebGazebo::GetPose(std::string model, gazebo::Pose& pose)
-{
// Discard any leftover responses
this->simIface->data->responseCount = 0;
// Ask Gazebo
- this->simIface->GetPose3d(model.c_str());
+ this->simIface->GetPose3d(name.c_str());
// Wait for the response
double timeout = 3.0;
@@ -122,55 +94,59 @@
if(((t1.tv_sec + t1.tv_usec/1e6) - (t0.tv_sec + t0.tv_usec/1e6))
> timeout)
{
- puts("Timeout");
+ error= "Timeout";
return false;
}
nanosleep(&sleeptime, NULL);
}
assert(this->simIface->data->responseCount == 1);
- pose = this->simIface->data->responses[0].modelPose;
+ p.x = this->simIface->data->responses[0].modelPose.pos.x;
+ p.y = this->simIface->data->responses[0].modelPose.pos.y;
+ p.z = this->simIface->data->responses[0].modelPose.pos.z;
+ p.r = this->simIface->data->responses[0].modelPose.roll;
+ p.p = this->simIface->data->responses[0].modelPose.pitch;
+ p.a = this->simIface->data->responses[0].modelPose.yaw;
return true;
}
bool
-WebGazebo::SetPose(std::string model, const gazebo::Pose& pose)
+WebGazebo::SetModelPVA(const std::string& name,
+ const websim::Pose& p,
+ const websim::Velocity& v,
+ const websim::Acceleration& a,
+ std::string& error)
{
// TODO: get pose, check for too-large jump, and complain about it
// Tell Gazebo the new pose
- this->simIface->SetPose3d(model.c_str(), pose);
+ gazebo::Pose pose;
+ gazebo::Vec3 lv, av, la, aa;
+ pose.pos.x = p.x;
+ pose.pos.y = p.y;
+ pose.pos.z = p.z;
+ pose.roll = p.r;
+ pose.pitch = p.p;
+ pose.yaw = p.a;
- return true;
-}
+ lv.x = v.x;
+ lv.y = v.y;
+ lv.z = v.z;
+ av.x = v.r;
+ av.y = v.p;
+ av.z = v.a;
-bool
-WebGazebo::ParseURI(std::string& model,
- std::string& prop,
- std::string& action,
- std::string uri,
- std::string& response)
-{
- // Remove the query args
- std::vector<std::string> uri_parts;
- StringSplit(uri, uri_parts, "?");
- assert(uri_parts.size() > 0);
- std::string bare_uri = uri_parts[0];
+ la.x = a.x;
+ la.y = a.y;
+ la.z = a.z;
+ aa.x = a.r;
+ aa.y = a.p;
+ aa.z = a.a;
- // We require 3 path components: model/property/action
- StringSplit(bare_uri, uri_parts, "/");
- // There should be 4 parts, with the first one empty
- if(uri_parts.size() != 4)
- {
- response = "Must be 3 slash-separated parts in the URI";
- return false;
- }
+ this->simIface->SetState(name.c_str(),
+ pose, lv, av, la, aa);
- model = uri_parts[1];
- prop = uri_parts[2];
- action = uri_parts[3];
-
return true;
}
@@ -248,9 +224,14 @@
}
bool
-WebGazebo::CreateModel(const std::string& xmldata,
- std::string& response)
+WebGazebo::CreateModel(const std::string& name,
+ const std::string& type,
+ std::string& error)
{
+ std::string xmldata;
+ if(!GetModel(name,type,xmldata,error))
+ return false;
+
struct timespec sleeptime = {0, 10000000};
for(;;)
{
@@ -270,279 +251,9 @@
}
bool
-WebGazebo::HandleSimRequest(const std::string& prop,
- const std::string& action,
- struct evkeyvalq* kv,
- std::string& response)
+WebGazebo::DeleteModel(const std::string& name,
+ std::string& error)
{
- // 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;
- }
-
- std::string xmldata;
- if(!GetModel(name, type, xmldata, response))
- return false;
- if(!CreateModel(xmldata, response))
- return false;
- response = std::string("Created model of type ") + type;
- return true;
- }
- else
- {
- response = "ERROR: Unknown action " + action + " for sim/factory";
- return false;
- }
- }
- else
- {
- response = "ERROR: Unknown property " + prop + " for sim";
- return false;
- }
-}
-
-bool
-WebGazebo::HandleModelRequest(const std::string& model,
- const std::string& prop,
- const std::string& action,
- struct evkeyvalq* kv,
- std::string& response)
-{
- /*
- printf("[webgazebo] got request for %s:%s:%s\n",
- model.c_str(),
- prop.c_str(),
- action.c_str());
- */
- if(HasModel(model))
- {
- if(action == "get")
- {
- if(prop == "pose")
- {
- gazebo::Pose p;
- if(GetPose(model, p))
- {
- char buf[1024];
- snprintf(buf, sizeof(buf),
- "Got %s's pose: (%.3f,%.3f,%.3f) (%.3f,%.3f,%.3f)",
- model.c_str(),
- p.pos.x, p.pos.y, p.pos.z,
- p.roll, p.pitch, p.yaw);
- 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
- gazebo::Pose p, q;
- if(!GetPose(model, p))
- {
- response = "Failed to get pose before setting it";
- return false;
- }
- try
- {
- q = p;
- if(GetValue(sx, kv, "x"))
- p.pos.x = boost::lexical_cast<float>(sx);
- if(GetValue(sy, kv, "y"))
- p.pos.y = boost::lexical_cast<float>(sy);
- if(GetValue(sz, kv, "z"))
- p.pos.z = boost::lexical_cast<float>(sz);
- if(GetValue(sroll, kv, "roll"))
- p.roll = boost::lexical_cast<float>(sroll);
- if(GetValue(spitch, kv, "pitch"))
- p.pitch = boost::lexical_cast<float>(spitch);
- if(GetValue(syaw, kv, "yaw"))
- p.yaw = boost::lexical_cast<float>(syaw);
- }
- catch(boost::bad_lexical_cast e)
- {
- response = std::string("Failed to parse input value(s): ") +
- e.what();
- return false;
- }
-
- if(SetPose(model, p))
- {
- char buf[1024];
- snprintf(buf, sizeof(buf),
- "Set %s's pose: (%.3f,%.3f,%.3f) (%.3f,%.3f,%.3f)",
- model.c_str(),
- p.pos.x, p.pos.y, p.pos.z,
- p.roll, p.pitch, p.yaw);
- response = buf;
-
- // Check tolerances, and complain if they're exceeded
- if(!CheckTolerances(p,q))
- response += "\nWARNING: Pose change exceeded tolerance";
-
- 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
- {
- response = "ERROR: Unknown action " + action;
- return false;
- }
- }
- else
- {
- response = "ERROR: No such model " + model;
- return false;
- }
-}
-
-bool
-WebGazebo::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);
- }
-}
-
-// Handle all incoming URI requests
-void
-WebGazebo::EventCallback(evhttp_request* req, void* arg)
-{
- WebGazebo* obj = (WebGazebo*)arg;
-
- /*
- printf("[webgazebo] Got request:%s:\n", req->uri);
- */
-
- // Pull out query args
- struct evkeyvalq query_args;
- evhttp_parse_query(req->uri, &query_args);
-
- struct evbuffer* eb = evbuffer_new();
- assert(eb);
-
- int response_code;
- std::string response_string;
- std::string model, prop, action, response;
- if(!obj->ParseURI(model, prop, action, req->uri, response))
- {
- response_code = 400;
- response_string = "Invalid request";
- }
- else if(!obj->HandleURI(model, prop, action, &query_args, response))
- {
- response_code = 400;
- response_string = "Invalid request";
- }
- else
- {
- response_code = 200;
- response_string = "OK";
- }
-
- evbuffer_add_printf(eb, "%s\n", response.c_str());
- /*
- printf("[webgazebo] Sending reply: %d %s\n",
- response_code, response_string.c_str());
- */
- evhttp_send_reply(req, response_code, response_string.c_str(), eb);
- evbuffer_free(eb);
-
- obj->DeleteKeyVal(&query_args);
-}
-
-void
-WebGazebo::Spin()
-{
- event_dispatch();
-}
-
-void
-WebGazebo::StringSplit(const std::string &s,
- std::vector<std::string> &t,
- const std::string &d)
-{
- t.clear();
- size_t start = 0, end;
- while ((end = s.find_first_of(d, start)) != std::string::npos)
- {
- t.push_back(s.substr(start, end-start));
- start = end + 1;
- }
- t.push_back(s.substr(start));
-}
-
-// Strange that libevent doesn't provide a way to free the evkeyvalq
-// structure.
-void
-WebGazebo::DeleteKeyVal(struct evkeyvalq* query_args)
-{
- struct evkeyval* kv;
- TAILQ_FOREACH(kv, query_args, next)
- {
- free(kv->key);
- free(kv->value);
- TAILQ_REMOVE(query_args, kv, next);
- }
-}
-
-bool
-WebGazebo::GetValue(std::string& value,
- struct evkeyvalq* query_args,
- const std::string& key)
-{
- struct evkeyval* kv;
- TAILQ_FOREACH(kv, query_args, next)
- {
- if(key == kv->key)
- {
- value = std::string(kv->value);
- return true;
- }
- }
+ error = "DeleteModel not implemented";
return false;
}
Modified: code/branches/federation/gazebo/webgazebo/WebGazebo.hh
===================================================================
--- code/branches/federation/gazebo/webgazebo/WebGazebo.hh 2009-03-10
05:43:37 UTC (rev 7426)
+++ code/branches/federation/gazebo/webgazebo/WebGazebo.hh 2009-03-10
05:48:01 UTC (rev 7427)
@@ -26,37 +26,46 @@
*/
#include <string>
-#include <vector>
#include <map>
// These headers must be included prior to the libevent headers
#include <sys/types.h>
#include <sys/queue.h>
-// libevent
-#include <event.h>
-#include <evhttp.h>
-
// libgazebo; the libgazebo directory is used because we're building
// against the source tree
#include <libgazebo/gazebo.h>
-class WebGazebo
+#include "websim/websim.h"
+
+class WebGazebo : public websim::WebSim
{
public:
- WebGazebo(std::string _host, int _port,
+ WebGazebo(const std::string& fedfile,
+ const std::string& host, int port,
double dtol, double atol);
- ~WebGazebo();
+ virtual ~WebGazebo();
+
+ // Interface to be implemented by simulators
+ virtual bool CreateModel(const std::string& name,
+ const std::string& type,
+ std::string& error);
+ virtual bool DeleteModel(const std::string& name,
+ std::string& error);
+ virtual bool SetModelPVA(const std::string& name,
+ const websim::Pose& p,
+ const websim::Velocity& v,
+ const websim::Acceleration& a,
+ std::string& error);
+ virtual bool GetModelPVA(const std::string& name,
+ websim::Pose& p,
+ websim::Velocity& v,
+ websim::Acceleration& a,
+ std::string& error);
- void Spin();
-
private:
- std::string host;
- int port;
double sq_dist_tol, sq_ang_tol;
- struct evhttp* eh;
-
gazebo::Client *client;
gazebo::SimulationIface *simIface;
gazebo::FactoryIface *factoryIface;
@@ -64,44 +73,9 @@
// Available models
std::map<std::string,int> models;
- // Static, so that it can be passed as a callback to libevent
- static void EventCallback(evhttp_request* req, void* arg);
-
bool CheckTolerances(gazebo::Pose p, gazebo::Pose q);
- bool GetModelList();
- bool HasModel(const std::string& name);
- bool CreateModel(const std::string& xmldata,
- std::string& response);
- bool GetPose(std::string model, gazebo::Pose& pose);
- bool SetPose(std::string model, const gazebo::Pose& pose);
- void StringSplit(const std::string &s,
- std::vector<std::string> &t,
- const std::string &d);
- bool GetValue(std::string& value,
- struct evkeyvalq* query_args,
- const std::string& key);
bool GetModel(const std::string& name,
const std::string& type,
std::string& xmldata,
std::string& response);
- bool HandleURI(const std::string& model,
- const std::string& prop,
- const std::string& action,
- struct evkeyvalq* kv,
- std::string& response);
- bool HandleSimRequest(const std::string& prop,
- const std::string& action,
- struct evkeyvalq* kv,
- std::string& response);
- bool HandleModelRequest(const std::string& model,
- const std::string& prop,
- const std::string& action,
- struct evkeyvalq* kv,
- std::string& response);
- bool ParseURI(std::string& model,
- std::string& prop,
- std::string& action,
- std::string uri,
- std::string& response);
- void DeleteKeyVal(struct evkeyvalq* query_args);
};
Modified: code/branches/federation/gazebo/webgazebo/main.cc
===================================================================
--- code/branches/federation/gazebo/webgazebo/main.cc 2009-03-10 05:43:37 UTC
(rev 7426)
+++ code/branches/federation/gazebo/webgazebo/main.cc 2009-03-10 05:48:01 UTC
(rev 7427)
@@ -29,13 +29,14 @@
#include <stdlib.h>
-#define USAGE "USAGE: webgazebo [-h <host>] [-p <port>]\n"
+#define USAGE "USAGE: webgazebo -f <file.fed> [-h <host>] [-p <port>] [-d
<dtol>] [-a <atol>]\n"
#define DEFAULT_PORT 8000
#define DEFAULT_HOST "localhost"
int g_port = DEFAULT_PORT;
std::string g_host = DEFAULT_HOST;
+std::string g_fedfile;
double g_dtol, g_atol;
bool ParseArgs(int argc, char** argv);
@@ -51,8 +52,8 @@
try
{
- WebGazebo wg(g_host, g_port, g_dtol, g_atol);
- wg.Spin();
+ WebGazebo wg(g_fedfile, g_host, g_port, g_dtol, g_atol);
+ wg.Update();
}
catch(std::string e)
{
@@ -66,13 +67,19 @@
bool
ParseArgs(int argc, char** argv)
{
- char *flags = (char*)("p:h:d:a:");
+ char *flags = (char*)("f:p:h:d:a:");
int ch;
while ((ch = getopt(argc, argv, flags)) != -1)
{
switch(ch)
{
+ // federation file
+ case 'f':
+ if(!optarg)
+ return false;
+ g_fedfile = optarg;
+ break;
// port
case 'p':
if(!optarg)
@@ -102,7 +109,7 @@
}
}
- if((g_host.size() == 0) || (g_port == 0))
+ if((g_fedfile.size() == 0) || (g_host.size() == 0) || (g_port == 0))
return false;
return true;
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
