Revision: 7710
http://playerstage.svn.sourceforge.net/playerstage/?rev=7710&view=rev
Author: asadat
Date: 2009-05-22 21:57:22 +0000 (Fri, 22 May 2009)
Log Message:
-----------
Laser and sonar messages is added to the websim interface. Also, the response
can be in XML format.
Modified Paths:
--------------
code/websim/CMakeLists.txt
code/websim/src/websim.cc
code/websim/src/websim.hh
Modified: code/websim/CMakeLists.txt
===================================================================
--- code/websim/CMakeLists.txt 2009-05-22 17:23:31 UTC (rev 7709)
+++ code/websim/CMakeLists.txt 2009-05-22 21:57:22 UTC (rev 7710)
@@ -39,9 +39,19 @@
MESSAGE( ${INDENT} "Glib not detected" )
ENDIF( GLIB_FOUND )
-include_directories( ${GLIB_INCLUDE_DIRS} )
-link_directories(${GLIB_LIBRARY_DIRS} )
+pkg_search_module( LIBXML2 REQUIRED libxml-2.0)
+IF( LIBXML2_FOUND )
+ MESSAGE( STATUS ${INDENT} "libxml version ${LIBXML2_VERSION} detected at
${LIBXML2_PREFIX}" )
+ELSE( LIBXML2_FOUND )
+ MESSAGE( ${INDENT} "libxml2 not detected" )
+ENDIF( LIBXML2_FOUND )
+include_directories( ${GLIB_INCLUDE_DIRS}
+ ${LIBXML2_INCLUDE_DIRS} )
+link_directories(${GLIB_LIBRARY_DIRS}
+ ${LIBXML2_LIBRARY_DIRS} )
+
+
include_directories(src)
add_library(websim SHARED src/websim.cc src/parser.cc src/confederate.cc
src/puppet.cc )
add_library(websim-static STATIC src/websim.cc src/parser.cc
src/confederate.cc src/puppet.cc )
@@ -50,8 +60,9 @@
# Prevent deletion of existing lib of same name
set_target_properties(websim-static PROPERTIES CLEAN_DIRECT_OUTPUT 1)
-target_link_libraries(websim event ${GLIB_LIBRARIES} )
+target_link_libraries(websim event ${GLIB_LIBRARIES} ${LIBXML2_LIBRARIES})
+
install(TARGETS websim websim-static
LIBRARY DESTINATION lib
ARCHIVE DESTINATION lib)
Modified: code/websim/src/websim.cc
===================================================================
--- code/websim/src/websim.cc 2009-05-22 17:23:31 UTC (rev 7709)
+++ code/websim/src/websim.cc 2009-05-22 21:57:22 UTC (rev 7710)
@@ -31,12 +31,19 @@
#include <assert.h>
#include <stdlib.h>
+#include <string.h>
+#include <libxml/encoding.h>
+#include <libxml/xmlwriter.h>
+#define MY_ENCODING "ISO-8859-1"
+
using namespace websim;
const std::string WebSim::package = "WebSim";
const std::string WebSim::version = "0.1";
+xmlChar *
+ConvertInput(const char *in, const char *encoding);
std::string Time::String()
{
@@ -272,17 +279,8 @@
Time t;
if(GetModelPVA(model, t, p, v, a, response))
{
- char buf[1024];
- snprintf(buf, sizeof(buf),
- "%s's state @%s: \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(),
- t.String().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;
+ GetTextPVA(model, t, p, v, a, response);
+
return true;
}
else
@@ -290,6 +288,79 @@
response = "ERROR: Failed to get pose for model " + model;
return false;
}
+ }else if(prop == "laser"){
+ Time t;
+ uint32_t res;
+ double fov;
+ Pose p;
+ std::vector<double> ranges;
+ if(GetLaserData(model, t, res, fov, p,ranges, response))
+ {
+ GetTextLaserData(model, t, res, fov, p, ranges, response);
+ return true;
+
+ }
+ else
+ {
+ response = "ERROR: Failed to get laser data for model " + model;
+ return false;
+ }
+
+ }else if(prop=="lasercfg"){
+
+ Time t;
+ uint32_t res;
+ double fov;
+ Pose p;
+
+ if(GetLaserCfgData(model, t, res, fov, p, response))
+ {
+
+ GetTextLaserCfgData(model, t, res, fov, p, response);
+ return true;
+
+ }
+ else
+ {
+ response = "ERROR: Failed to get laser cfg data for model " + model;
+ return false;
+ }
+
+ }else if(prop=="ranger"){
+
+ Time t;
+ std::vector<double> ranges;
+
+ if(GetRangerData(model, t, ranges, response))
+ {
+
+ GetTextRangerData(model, t, ranges, response);
+ return true;
+
+ }
+ else
+ {
+ response = "ERROR: Failed to get ranger data for model " + model;
+ return false;
+ }
+
+ }else if(prop=="rangercfg"){
+
+ Time t;
+ std::vector<Pose> p;
+
+ if(GetRangerCfgData(model, t, p, response))
+ {
+
+ GetTextRangerCfgData(model, t, p, response);
+ return true;
+
+ }
+ else
+ {
+ response = "ERROR: Failed to get ranger Cfg data for model " + model;
+ return false;
+ }
}
else
{
@@ -376,7 +447,459 @@
}
}
+
+
+
+void
+WebSim::GetTextPVA(const std::string& name,
+ Time& t,
+ const
Pose& p,
+ const
Velocity& v,
+ const
Acceleration& a,
+
std::string& response)
+{
+
+ char buf[1024];
+ snprintf(buf, sizeof(buf),
+ "%s's state @%s: \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",
+ name.c_str(),
+ t.String().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;
+
+}
+
+
+void
+WebSim::GetXMLPVA(const std::string& name,
+ Time& t,
+ const
Pose& p,
+ const
Velocity& v,
+ const
Acceleration& a,
+
std::string& response)
+{
+
+ xmlTextWriterPtr writer;
+ xmlBufferPtr buf;
+ xmlChar *tmp;
+ char str[32];
+
+ buf = xmlBufferCreate();
+ writer = xmlNewTextWriterMemory(buf, 0);
+
+
+ xmlTextWriterStartElement(writer, BAD_CAST "Data");
+
+ sprintf(str,"%s",t.String().c_str());
+ tmp = ConvertInput(str,MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "Time", BAD_CAST tmp);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "Type", BAD_CAST "pva");
+
+ tmp = ConvertInput(name.c_str(),MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "Model", BAD_CAST tmp);
+
+ xmlTextWriterStartElement(writer, BAD_CAST "Pose");
+
+ sprintf(str,"%.3f",p.x);
+ tmp = ConvertInput(str, MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "x", BAD_CAST tmp);
+
+ sprintf(str,"%.3f",p.y);
+ tmp = ConvertInput(str, MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "y", BAD_CAST tmp);
+
+ sprintf(str,"%.3f",p.z);
+ tmp = ConvertInput(str, MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "z", BAD_CAST tmp);
+
+ sprintf(str,"%.3f",p.r);
+ tmp = ConvertInput(str, MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "r", BAD_CAST tmp);
+
+ sprintf(str,"%.3f",p.p);
+ tmp = ConvertInput(str, MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "p", BAD_CAST tmp);
+
+ sprintf(str,"%.3f",p.a);
+ tmp = ConvertInput(str, MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "a", BAD_CAST tmp);
+
+ xmlTextWriterEndElement(writer);
+
+
+
+ xmlTextWriterStartElement(writer, BAD_CAST "Velocity");
+
+ sprintf(str,"%.3f",v.x);
+ tmp = ConvertInput(str, MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "x", BAD_CAST tmp);
+
+ sprintf(str,"%.3f",v.y);
+ tmp = ConvertInput(str, MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "y", BAD_CAST tmp);
+
+ sprintf(str,"%.3f",v.z);
+ tmp = ConvertInput(str, MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "z", BAD_CAST tmp);
+
+ sprintf(str,"%.3f",v.r);
+ tmp = ConvertInput(str, MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "r", BAD_CAST tmp);
+
+ sprintf(str,"%.3f",v.p);
+ tmp = ConvertInput(str, MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "p", BAD_CAST tmp);
+
+ sprintf(str,"%.3f",v.a);
+ tmp = ConvertInput(str, MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "a", BAD_CAST tmp);
+
+ xmlTextWriterEndElement(writer);
+
+
+
+
+ xmlTextWriterStartElement(writer, BAD_CAST "Acceleration");
+
+ sprintf(str,"%.3f",a.x);
+ tmp = ConvertInput(str, MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "x", BAD_CAST tmp);
+
+ sprintf(str,"%.3f",a.y);
+ tmp = ConvertInput(str, MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "y", BAD_CAST tmp);
+
+ sprintf(str,"%.3f",a.z);
+ tmp = ConvertInput(str, MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "z", BAD_CAST tmp);
+
+ sprintf(str,"%.3f",a.r);
+ tmp = ConvertInput(str, MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "r", BAD_CAST tmp);
+
+ sprintf(str,"%.3f",a.p);
+ tmp = ConvertInput(str, MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "p", BAD_CAST tmp);
+
+ sprintf(str,"%.3f",a.a);
+ tmp = ConvertInput(str, MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "a", BAD_CAST tmp);
+
+ xmlTextWriterEndElement(writer);
+
+ xmlTextWriterEndElement(writer);
+
+ xmlTextWriterEndDocument(writer);
+ xmlFreeTextWriter(writer);
+
+
+ puts((const char*) buf->content);
+
+ response = (const char*) buf->content;
+
+ xmlBufferFree(buf);
+
+
+
+}
+
void
+WebSim::GetTextLaserData(const std::string& name,
+ Time& t,
+ uint32_t&
resolution,
+ double& fov,
+ Pose& p,
+
std::vector<double>& ranges,
+ std::string&
response)
+{
+ std::string res;
+ char temp[128];
+ for(unsigned int i=0;i<ranges.size();i++){
+ sprintf(temp,"%.3f",ranges.at(i));
+ res.append(temp);
+ if(i+1 != ranges.size())
+ res.append(",");
+ }
+
+
+ char buf[2048];
+ snprintf(buf, sizeof(buf),
+ "%s's state @%s: \n laser: (%s)\n resolution: (%d)\n
fov(%.2f)\n"
+ " pose (%.3f,%.3f,%.3f) (%.3f,%.3f,%.3f)\n",
+ name.c_str(),
+ t.String().c_str(),
+ res.c_str(),
+ resolution,
+ fov,
+ p.x,p.y,p.z,p.r,p.p,p.a);
+ response = buf;
+}
+
+void
+WebSim::GetXMLLaserData(const std::string& name,
+ Time& t,
+ uint32_t&
resolution,
+ double& fov,
+ Pose& p,
+
std::vector<double>& ranges,
+ std::string&
response)
+{
+
+
+ xmlTextWriterPtr writer;
+ xmlBufferPtr buf;
+ xmlChar *tmp;
+ char str[32];
+
+
+ std::string res;
+ char temp[128];
+ for(unsigned int i=0;i<ranges.size();i++){
+ sprintf(temp,"%.3f",ranges.at(i));
+ res.append(temp);
+ if(i+1 != ranges.size())
+ res.append(",");
+ }
+
+
+ buf = xmlBufferCreate();
+ writer = xmlNewTextWriterMemory(buf, 0);
+
+
+ xmlTextWriterStartElement(writer, BAD_CAST "Data");
+
+ sprintf(str,"%s",t.String().c_str());
+ tmp = ConvertInput(str,MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "Time", BAD_CAST tmp);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "Type", BAD_CAST "laser");
+
+ tmp = ConvertInput(name.c_str(),MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "Model", BAD_CAST tmp);
+
+ xmlTextWriterStartElement(writer, BAD_CAST "Laser");
+
+ sprintf(str,"%d",resolution);
+ tmp = ConvertInput(str,MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "Resolution", BAD_CAST tmp);
+
+ sprintf(str,"%.3f",fov);
+ tmp = ConvertInput(str,MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "FOV", BAD_CAST tmp);
+
+ sprintf(str,"%.3f,%.3f,%.3f,%.3f,%.3f,%.3f",p.x, p.y, 0.f, 0.f, 0.f, p.a);
+ tmp = ConvertInput(str,MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "Pose", BAD_CAST tmp);
+
+ tmp = ConvertInput(res.c_str(), MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "Samples", BAD_CAST tmp);
+
+
+ xmlTextWriterEndElement(writer);
+
+ xmlTextWriterEndElement(writer);
+
+ xmlTextWriterEndDocument(writer);
+ xmlFreeTextWriter(writer);
+
+
+ puts((const char*) buf->content);
+
+ response = (const char*) buf->content;
+
+ xmlBufferFree(buf);
+
+}
+void
+WebSim::GetTextLaserCfgData(const std::string& name,
+ Time& t,
+
uint32_t& resolution,
+ double&
fov,
+ Pose& p,
+
std::string& response)
+{
+ char buf[2048];
+ snprintf(buf, sizeof(buf),
+ "%s's state @%s: \n resolution(%d)\n fov(%.2f)\n"
+ " pose (%.3f,%.3f,%.3f) (%.3f,%.3f,%.3f)\n",
+ name.c_str(),
+ t.String().c_str(),
+ resolution,
+ fov,
+ p.x,p.y,p.z,p.r,p.p,p.a);
+ response = buf;
+}
+void
+WebSim::GetTextRangerData(const std::string& name,
+ Time& t,
+
std::vector<double>& ranges,
std::string& response)
+{
+ std::string res;
+ char temp[128];
+ for(unsigned int i=0;i<ranges.size();i++){
+ sprintf(temp,"%.3f",ranges.at(i));
+ res.append(temp);
+ if(i+1 != ranges.size())
+ res.append(",");
+ }
+
+
+ char buf[2048];
+ snprintf(buf, sizeof(buf),
+ "%s's state @%s: \n ranger: (%s)\n",
+ name.c_str(),
+ t.String().c_str(),
+ res.c_str());
+ response = buf;
+}
+
+void
+WebSim::GetXMLRangerData(const std::string& name,
+ Time& t,
+
std::vector<double>& ranges,
std::string& response)
+{
+
+ xmlTextWriterPtr writer;
+ xmlBufferPtr buf;
+ xmlChar *tmp;
+ char str[32];
+
+
+ std::string res;
+ char temp[128];
+ for(unsigned int i=0;i<ranges.size();i++){
+ sprintf(temp,"%.3f",ranges.at(i));
+ res.append(temp);
+ if(i+1 != ranges.size())
+ res.append(",");
+ }
+
+
+ buf = xmlBufferCreate();
+ writer = xmlNewTextWriterMemory(buf, 0);
+
+
+ xmlTextWriterStartElement(writer, BAD_CAST "Data");
+
+ sprintf(str,"%s",t.String().c_str());
+ tmp = ConvertInput(str,MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "Time", BAD_CAST tmp);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "Type", BAD_CAST "ranger");
+
+ tmp = ConvertInput(name.c_str(),MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "Model", BAD_CAST tmp);
+
+ xmlTextWriterStartElement(writer, BAD_CAST "Ranger");
+
+
+ tmp = ConvertInput(res.c_str(), MY_ENCODING);
+ xmlTextWriterWriteAttribute(writer, BAD_CAST "samples", BAD_CAST tmp);
+
+
+ xmlTextWriterEndElement(writer);
+
+ xmlTextWriterEndElement(writer);
+
+ xmlTextWriterEndDocument(writer);
+ xmlFreeTextWriter(writer);
+
+
+ puts((const char*) buf->content);
+
+ response = (const char*) buf->content;
+
+ xmlBufferFree(buf);
+
+
+}
+
+
+void
+WebSim::GetTextRangerCfgData(const std::string& name,
+ Time& t,
+
std::vector<Pose>& p,
+
std::string& response)
+{
+
+ std::string res;
+ char temp[256];
+ for(unsigned int i=0;i<p.size();i++){
+ Pose pos = p.at(i);
+ sprintf(temp," (%.3f,%.3f,%.3f) (%.3f,%.3f,%.3f)",
+ pos.x,pos.y,pos.x,pos.r,pos.p,pos.a);
+ res.append(temp);
+
+ }
+
+
+ char buf[2048];
+ snprintf(buf, sizeof(buf),
+ "%s's state @%s: \n rangerCfg:Pose (%s)\n",
+ name.c_str(),
+ t.String().c_str(),
+ res.c_str());
+ response = buf;
+
+
+
+}
+
+xmlChar *
+ConvertInput(const char *in, const char *encoding)
+{
+ xmlChar *out;
+ int ret;
+ int size;
+ int out_size;
+ int temp;
+ xmlCharEncodingHandlerPtr handler;
+
+ if (in == 0)
+ return 0;
+
+ handler = xmlFindCharEncodingHandler(encoding);
+
+ if (!handler) {
+ printf("ConvertInput: no encoding handler found for '%s'\n",
+ encoding ? encoding : "");
+ return 0;
+ }
+
+ size = (int) strlen(in) + 1;
+ out_size = size * 2 - 1;
+ out = (unsigned char *) xmlMalloc((size_t) out_size);
+
+ if (out != 0) {
+ temp = size - 1;
+ ret = handler->input(out, &out_size, (const xmlChar *) in, &temp);
+ if ((ret < 0) || (temp - size + 1)) {
+ if (ret < 0) {
+ printf("ConvertInput: conversion wasn't successful.\n");
+ } else {
+ printf
+ ("ConvertInput: conversion wasn't successful. converted:
%i octets.\n",
+ temp);
+ }
+
+ xmlFree(out);
+ out = 0;
+ } else {
+ out = (unsigned char *) xmlRealloc(out, out_size + 1);
+ out[out_size] = 0;
+ }
+ } else {
+ printf("ConvertInput: no mem\n");
+ }
+
+ return out;
+}
+
+
+
+void
WebSim::EventCallback(evhttp_request* req, void* arg)
{
WebSim* obj = (WebSim*)arg;
Modified: code/websim/src/websim.hh
===================================================================
--- code/websim/src/websim.hh 2009-05-22 17:23:31 UTC (rev 7709)
+++ code/websim/src/websim.hh 2009-05-22 21:57:22 UTC (rev 7710)
@@ -82,6 +82,86 @@
Acceleration& a,
std::string& response) = 0;
+ void GetTextPVA(const std::string& name,
+ Time& t,
+ const
Pose& p,
+ const
Velocity& v,
+ const
Acceleration& a,
+
std::string& response);
+
+ void GetXMLPVA(const std::string& name,
+ Time& t,
+ const
Pose& p,
+ const
Velocity& v,
+ const
Acceleration& a,
+
std::string& response);
+ virtual bool GetLaserData(const std::string& name,
+ Time& t,
+
uint32_t& resolution,
+ double&
fov,
+ Pose& p,
+
std::vector<double>& ranges,
+
std::string& response) = 0;
+
+ void GetTextLaserData(const std::string& name,
+ Time& t,
+
uint32_t& resolution,
+ double&
fov,
+ Pose& p,
+
+
std::vector<double>& ranges,
+
std::string& response);
+
+
+ void GetXMLLaserData(const std::string& name,
+ Time& t,
+
uint32_t& resolution,
+ double&
fov,
+ Pose& p,
+
std::vector<double>& ranges,
+
std::string& response);
+
+ virtual bool GetLaserCfgData(const std::string& name,
+ Time& t,
+
uint32_t& resolution,
+ double&
fov,
+ Pose& p,
+
std::string& response) = 0;
+
+ void GetTextLaserCfgData(const std::string& name,
+ Time& t,
+
uint32_t& resolution,
+ double&
fov,
+ Pose& p,
+
std::string& response);
+
+ virtual bool GetRangerData(const std::string& name,
+ Time& t,
+
std::vector<double>& ranges,
+
std::string& response) = 0;
+
+
+ void GetTextRangerData(const std::string& name,
+ Time& t,
+
std::vector<double>& ranges,
+
std::string& response);
+
+
+ void GetXMLRangerData(const std::string& name,
+ Time& t,
+
std::vector<double>& ranges,
+
std::string& response);
+
+ virtual bool GetRangerCfgData(const std::string& name,
+ Time& t,
+
std::vector<Pose>& p,
+
std::string& response) = 0;
+
+ void GetTextRangerCfgData(const std::string& name,
+ Time& t,
+
std::vector<Pose>& p,
+
std::string& response);
+
/** Get the current simulation time */
virtual Time GetTime() = 0;
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Register Now for Creativity and Technology (CaT), June 3rd, NYC. CaT
is a gathering of tech-side developers & brand creativity professionals. Meet
the minds behind Google Creative Lab, Visual Complexity, Processing, &
iPhoneDevCamp asthey present alongside digital heavyweights like Barbarian
Group, R/GA, & Big Spaceship. http://www.creativitycat.com
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
