Revision: 7396
http://playerstage.svn.sourceforge.net/playerstage/?rev=7396&view=rev
Author: gerkey
Date: 2009-03-09 05:40:51 +0000 (Mon, 09 Mar 2009)
Log Message:
-----------
Got basic set and get working, with tolerance checking.
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
Modified: code/branches/federation/gazebo/webgazebo/SConscript
===================================================================
--- code/branches/federation/gazebo/webgazebo/SConscript 2009-03-09
02:59:16 UTC (rev 7395)
+++ code/branches/federation/gazebo/webgazebo/SConscript 2009-03-09
05:40:51 UTC (rev 7396)
@@ -21,11 +21,12 @@
CPPPATH=Split('#.')
)
-sources = Split('WebGazebo.cc')
+# We need Gazebo's quaternion functionality
+sources = Split('WebGazebo.cc #server/Quatern.os #server/Vector3.os
#server/Angle.os')
headers = Split('WebGazebo.hh')
sharedLib = libEnv.SharedLibrary('webgazebo', sources)
-staticLib = libEnv.StaticLibrary('webgazebo', sources)
+#staticLib = libEnv.StaticLibrary('webgazebo', ssources)
import os
@@ -50,5 +51,5 @@
webgazebo = exeEnv.Program('webgazebo', 'main.cc')
env.Install(install_prefix+'/lib', sharedLib)
-env.Install(install_prefix+'/lib', staticLib)
+#env.Install(install_prefix+'/lib', staticLib)
env.Install(install_prefix+'/bin', webgazebo)
Modified: code/branches/federation/gazebo/webgazebo/WebGazebo.cc
===================================================================
--- code/branches/federation/gazebo/webgazebo/WebGazebo.cc 2009-03-09
02:59:16 UTC (rev 7395)
+++ code/branches/federation/gazebo/webgazebo/WebGazebo.cc 2009-03-09
05:40:51 UTC (rev 7396)
@@ -5,8 +5,12 @@
#include <stdlib.h>
#include <time.h>
-WebGazebo::WebGazebo(std::string _host, int _port) :
- host(_host), port(_port)
+#include <boost/lexical_cast.hpp>
+
+#include "../server/Quatern.hh"
+
+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)
{
// Set up the HTTP server
// Not sure whether it's safe to do this more that once in one process
@@ -26,8 +30,11 @@
this->client = new gazebo::Client();
this->simIface = new gazebo::SimulationIface();
this->client->ConnectWait(0, GZ_CLIENT_ID_USER_FIRST);
- /// Open the Simulation Interface; let exceptions leak out
+ // Open the Simulation Interface; let exceptions leak out
this->simIface->Open(this->client, "default");
+ // Get the list of models
+ if(!GetModelList())
+ throw("Failed to get list of models from Gazebo");
puts("Done.");
puts("[webgazebo] Ready");
@@ -42,8 +49,22 @@
}
bool
-WebGazebo::GetPose(gazebo::Pose* pose, std::string model)
+WebGazebo::GetModelList()
{
+ // TODO
+ this->models["pioneer2dx_model1"] = 0;
+ return true;
+}
+
+bool WebGazebo::HasModel(const std::string& name)
+{
+ std::map<std::string,int>::const_iterator it = models.find(name);
+ return it != models.end();
+}
+
+bool
+WebGazebo::GetPose(std::string model, gazebo::Pose& pose)
+{
// Discard any leftover responses;
this->simIface->data->responseCount = 0;
@@ -56,24 +77,272 @@
nanosleep(&sleeptime, NULL);
assert(this->simIface->data->responseCount == 1);
- *pose = this->simIface->data->responses[0].modelPose;
+ pose = this->simIface->data->responses[0].modelPose;
return true;
}
+bool
+WebGazebo::SetPose(std::string model, const gazebo::Pose& pose)
+{
+ // TODO: get pose, check for too-large jump, and complain about it
+
+ // Tell Gazebo the new pose
+ this->simIface->SetPose3d(model.c_str(), pose);
+
+ return true;
+}
+
+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];
+
+ // 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;
+ }
+
+ model = uri_parts[1];
+ prop = uri_parts[2];
+ action = uri_parts[3];
+
+ return true;
+}
+
+bool
+WebGazebo::CheckTolerances(gazebo::Pose p, gazebo::Pose q)
+{
+ // Check position
+ double sq_dd = ((p.pos.x - q.pos.x)*(p.pos.x - q.pos.x) +
+ (p.pos.y - q.pos.y)*(p.pos.y - q.pos.y) +
+ (p.pos.z - q.pos.z)*(p.pos.z - q.pos.z));
+ if((sq_dist_tol > 0) && (sq_dd > sq_dist_tol))
+ return false;
+
+ // Check orientation
+ gazebo::Quatern p_quat, q_quat;
+ gazebo::Vector3 pv(p.roll, p.pitch, p.yaw);
+ gazebo::Vector3 qv(q.roll, q.pitch, q.yaw);
+ p_quat.SetFromEuler(pv);
+ q_quat.SetFromEuler(qv);
+ gazebo::Quatern da = p_quat - q_quat;
+ //da.Normalize();
+
+ double sq_da = ((da.u*da.u) +
+ (da.x*da.x) +
+ (da.y*da.y) +
+ (da.z*da.z));
+ if((sq_ang_tol > 0) && (sq_da > sq_ang_tol))
+ return false;
+
+ return true;
+}
+
+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")
+ {
+ // 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;
+ }
+
+ printf("[webgazebo] Creating model %s of type %s\n",
+ name.c_str(), type.c_str());
+ response = "[webgazebo] Creating model " + name + " 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;
+ }
+ }
+ // Everything else must be an existing model
+ else
+ {
+ 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
+ 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;
+ }
+ }
+}
+
void
WebGazebo::EventCallback(evhttp_request* req, void* arg)
{
WebGazebo* obj = (WebGazebo*)arg;
printf("[webgazebo] Got request:%s:\n", req->uri);
+ printf("[webgazebo] Decoded:%s:\n", evhttp_decode_uri(req->uri));
+
+
+ // Pull out query args
+ struct evkeyvalq query_args;
+ evhttp_parse_query(req->uri, &query_args);
+
struct evbuffer* eb = evbuffer_new();
assert(eb);
+ std::string model, prop, action, response;
+ if(!obj->ParseURI(model, prop, action, req->uri, response))
+ {
+ evbuffer_add_printf(eb, "%s\n", response.c_str());
+ evhttp_send_reply(req, 400, "Invalid request", eb);
+ return;
+ }
+
+ if(!obj->HandleURI(model, prop, action, &query_args, response))
+ {
+ evbuffer_add_printf(eb, "%s\n", response.c_str());
+ evhttp_send_reply(req, 400, "Invalid request", eb);
+ return;
+ }
+
+ /*
std::string model = "pioneer2dx_model1";
gazebo::Pose p;
assert(obj->GetPose(&p, model));
-
evbuffer_add_printf(eb, "<html><head><title>WebGazebo</title></head><body>");
evbuffer_add_printf(eb, "<h1>WebGazebo!</h1>");
evbuffer_add_printf(eb, "%s's pose: (%.3f,%.3f,%.3f) (%.3f,%.3f,%.3f)",
@@ -81,7 +350,9 @@
p.pos.x, p.pos.y, p.pos.z,
p.roll, p.pitch, p.yaw);
evbuffer_add_printf(eb, "</body></html>");
+ */
+ evbuffer_add_printf(eb, "%s\n", response.c_str());
evhttp_send_reply(req, 200, "OK", eb);
evbuffer_free(eb);
}
@@ -91,3 +362,38 @@
{
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));
+}
+
+bool
+WebGazebo::GetValue(std::string& value,
+ struct evkeyvalq* query_args,
+ const std::string& key)
+{
+ for(struct evkeyval* kv = ((struct evkeyval*)&query_args)->next.tqe_next;
+ kv;
+ kv = kv->next.tqe_next)
+ {
+ if(key == kv->key)
+ {
+ value = kv->value;
+ printf("setting %s to %s (%s)\n",
+ key.c_str(), kv->value, value.c_str());
+ return true;
+ }
+ }
+ return false;
+}
Modified: code/branches/federation/gazebo/webgazebo/WebGazebo.hh
===================================================================
--- code/branches/federation/gazebo/webgazebo/WebGazebo.hh 2009-03-09
02:59:16 UTC (rev 7395)
+++ code/branches/federation/gazebo/webgazebo/WebGazebo.hh 2009-03-09
05:40:51 UTC (rev 7396)
@@ -1,4 +1,6 @@
#include <string>
+#include <vector>
+#include <map>
// These headers must be included prior to the libevent headers
#include <sys/types.h>
@@ -15,7 +17,8 @@
class WebGazebo
{
public:
- WebGazebo(std::string _host, int _port);
+ WebGazebo(std::string _host, int _port,
+ double dtol, double atol);
~WebGazebo();
void Spin();
@@ -23,14 +26,38 @@
private:
std::string host;
int port;
+ double sq_dist_tol, sq_ang_tol;
struct evhttp* eh;
gazebo::Client *client;
gazebo::SimulationIface *simIface;
+ // 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 GetPose(gazebo::Pose* pose, std::string model);
+ bool CheckTolerances(gazebo::Pose p, gazebo::Pose q);
+ bool GetModelList();
+ bool HasModel(const std::string& name);
+ 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 HandleURI(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);
};
Modified: code/branches/federation/gazebo/webgazebo/main.cc
===================================================================
--- code/branches/federation/gazebo/webgazebo/main.cc 2009-03-09 02:59:16 UTC
(rev 7395)
+++ code/branches/federation/gazebo/webgazebo/main.cc 2009-03-09 05:40:51 UTC
(rev 7396)
@@ -9,6 +9,7 @@
int g_port = DEFAULT_PORT;
std::string g_host = DEFAULT_HOST;
+double g_dtol, g_atol;
bool ParseArgs(int argc, char** argv);
@@ -23,7 +24,7 @@
try
{
- WebGazebo wg(g_host, g_port);
+ WebGazebo wg(g_host, g_port, g_dtol, g_atol);
wg.Spin();
}
catch(std::string e)
@@ -38,7 +39,7 @@
bool
ParseArgs(int argc, char** argv)
{
- char *flags = (char*)("p:h:");
+ char *flags = (char*)("p:h:d:a:");
int ch;
while ((ch = getopt(argc, argv, flags)) != -1)
@@ -57,6 +58,18 @@
return false;
g_host = optarg;
break;
+ // distance tolerance
+ case 'd':
+ if(!optarg)
+ return false;
+ g_dtol = atof(optarg);
+ break;
+ // angular tolerance
+ case 'a':
+ if(!optarg)
+ return false;
+ g_atol = atof(optarg);
+ break;
default:
return false;
}
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Open Source Business Conference (OSBC), March 24-25, 2009, San Francisco, CA
-OSBC tackles the biggest issue in open source: Open Sourcing the Enterprise
-Strategies to boost innovation and cut costs with open source participation
-Receive a $600 discount off the registration fee with the source code: SFAD
http://p.sf.net/sfu/XcvMzF8H
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
