Tekkotsu Homepage
Demos
Overview
Downloads
Dev. Resources
Reference
Credits

StewartPlatformBehavior.cc

Go to the documentation of this file.
00001 #include "StewartPlatformBehavior.h"
00002 #include "Motion/EmergencyStopMC.h"
00003 #include "Motion/MMAccessor.h"
00004 #include "Wireless/Wireless.h"
00005 #include "Events/EventRouter.h"
00006 #include "Events/EventBase.h"
00007 
00008 #include <libxml/parser.h>
00009 
00010 StewartPlatformBehavior* StewartPlatformBehavior::theOne = NULL;
00011 
00012 void StewartPlatformBehavior::DoStart() {
00013   // Behavior startup
00014   BehaviorBase::DoStart();
00015   // Turn on wireless
00016   cmdsock=wireless->socket(Socket::SOCK_STREAM, 300, 300);
00017   sock=cmdsock->sock;
00018   wireless->setDaemon(cmdsock,true);
00019   wireless->setReceiver(cmdsock->sock, callback);
00020   wireless->listen(cmdsock->sock, config->main.stewart_port);
00021 }
00022 
00023 void StewartPlatformBehavior::DoStop() {
00024   // Turn off timers
00025   erouter->removeListener(this);
00026   // Close socket; turn wireless off
00027   wireless->setDaemon(cmdsock,false);
00028   closeComm();
00029   cmdsock=NULL;
00030   // Total behavior stop
00031   BehaviorBase::DoStop();
00032 }
00033 
00034 void StewartPlatformBehavior::setParam(const std::string& cmd, float value) {
00035   //cout << "got " << cmd << ' ' << value << endl;
00036   if(cmd=="x") {
00037   } else if(cmd=="y") {
00038   } else if(cmd=="z") {
00039   } else if(cmd=="tilt") {
00040   } else if(cmd=="pan") {
00041   } else if(cmd=="roll") {
00042   } else {
00043     serr->printf("StewartPlatformBehavior::runCommand() - bad message: '%s' == %g\n",cmd.c_str(),value);
00044   }
00045 }
00046 
00047 void StewartPlatformBehavior::processEvent(const EventBase & /*e*/) {
00048 }
00049 
00050 void* StewartPlatformBehavior::getSAXHandler() {
00051   static xmlSAXHandler handler;
00052   static bool isinit=false;
00053   if(!isinit) {
00054     memset(&handler,0,sizeof(handler));
00055     handler.endDocument  = (endDocumentSAXFunc)  StewartPlatformBehavior::sax_end_doc;
00056     handler.startElement = (startElementSAXFunc) StewartPlatformBehavior::sax_start_element;
00057     handler.endElement   = (endElementSAXFunc)   StewartPlatformBehavior::sax_end_element;
00058     handler.warning      = (warningSAXFunc)      StewartPlatformBehavior::sax_warning;
00059     handler.error        = (errorSAXFunc)        StewartPlatformBehavior::sax_error;
00060     handler.fatalError   = (fatalErrorSAXFunc)   StewartPlatformBehavior::sax_fatal_error;
00061     isinit=true;
00062   }
00063   return &handler;
00064 }
00065 
00066 // The command packet reassembly mechanism
00067 int StewartPlatformBehavior::callback(char *buf, int bytes) {
00068   if(StewartPlatformBehavior::theOne==NULL)
00069     return 0;
00070   
00071   static xmlSAXHandler * communicationParser = (xmlSAXHandler*)getSAXHandler();
00072 
00073   buf[bytes]='\0';
00074   theOne->docClosed=false;
00075   if(theOne->ctxt==NULL) {
00076     theOne->ctxt = xmlCreatePushParserCtxt(communicationParser, theOne, buf, bytes, NULL);
00077     if (theOne->ctxt == NULL) {
00078       serr->printf("StewartPlatform could not create parsing context\n");
00079       theOne->closeComm();
00080       return 1;
00081     }
00082     //this will trigger libxml to actually do processing on buf, *right now*
00083     if(xmlParseChunk((xmlParserCtxtPtr)theOne->ctxt,NULL,0,0)!=0) {
00084       serr->printf("StewartPlatform communication encountered an initial parsing error\n");
00085       serr->printf("closing StewartPlatformBehavior communication.\n");
00086       theOne->closeComm();
00087       return 1;
00088     }
00089   } else {
00090     if(xmlParseChunk((xmlParserCtxtPtr)theOne->ctxt,buf,bytes,0)!=0) {
00091       serr->printf("StewartPlatform communication encountered a parsing error\n");
00092       serr->printf("closing StewartPlatformBehavior communication.\n");
00093       theOne->closeComm();
00094       return 1;
00095     }
00096   }
00097   if(theOne->docClosed) {
00098     theOne->closeComm();
00099   }
00100   return 0;
00101 }
00102 
00103 void StewartPlatformBehavior::sax_start_element(StewartPlatformBehavior * inst, const char * name, const char ** attrs) {
00104   /*cout << "Receiving " << name << " with attributes: ";
00105   if(attrs!=NULL) {
00106     while(*attrs!=NULL)
00107       cout << *attrs++ << ' ';
00108   }
00109   cout << endl; */
00110   if(strcmp(name,"param")==0) {
00111     ASSERTRET(attrs!=NULL,"StewartPlatformBehavior: parser error, param missing attributes");
00112     const char * pname=NULL;
00113     const char * val=NULL;
00114     for(; *attrs!=NULL; attrs++) {
00115       if(strcmp(*attrs,"name")==0)
00116         pname=*++attrs;
00117       else if(strcmp(*attrs,"value")==0)
00118         val=*++attrs;
00119       else
00120         serr->printf("StewartPlatformBehavior: warning unknown attribute %s\n",*attrs++);
00121     }
00122     ASSERTRET(pname!=NULL,"StewartPlatformBehavior: parser error, param missing name attribute");
00123     ASSERTRET(val!=NULL,"StewartPlatformBehavior: parser error, param missing value attribute");
00124     inst->setParam(pname,atof(val));
00125   } else if(strcmp(name,"connection")==0) {
00126     if(inst!=NULL && inst->cmdsock!=NULL)
00127       inst->cmdsock->printf("<connection>\n");
00128   } 
00129 }
00130 void StewartPlatformBehavior::sax_end_element(StewartPlatformBehavior * inst, const char * name) {
00131   //cout << "end element " << name << endl;
00132   if(strcmp(name,"connection")==0) {
00133     inst->docClosed=true;
00134   }
00135 }
00136 
00137 void StewartPlatformBehavior::sax_end_doc(StewartPlatformBehavior * inst) {
00138   inst->docClosed=true;
00139 }
00140 
00141 void StewartPlatformBehavior::sax_warning(StewartPlatformBehavior * /*inst*/, const char *msg, ...) {
00142   va_list args;
00143   va_start(args, msg);
00144   serr->printf("StewartPlatformBehavior communication encountered a warning:\n");
00145   serr->vprintf(msg, args);
00146   va_end(args);
00147 }
00148 
00149 void StewartPlatformBehavior::sax_error(StewartPlatformBehavior * /*inst*/, const char *msg, ...) {
00150   va_list args;
00151   va_start(args, msg);
00152   serr->printf("StewartPlatformBehavior communication encountered an error:\n");
00153   serr->vprintf(msg, args);
00154   va_end(args);
00155 }
00156 
00157 void StewartPlatformBehavior::sax_fatal_error(StewartPlatformBehavior * /*inst*/, const char *msg, ...) {
00158   va_list args;
00159   va_start(args, msg);
00160   serr->printf("StewartPlatformBehavior communication encountered a fatal error:\n");
00161   serr->vprintf(msg, args);
00162   va_end(args);
00163 }
00164 
00165 bool StewartPlatformBehavior::isConnected() {
00166   bool con=theOne!=NULL && theOne->cmdsock!=NULL && wireless->isConnected(theOne->sock);
00167   //since we're using a daemon socket, we don't need to reset the cmdsock
00168   //if(!con)
00169   //theOne->cmdsock=NULL;
00170   if(!con && theOne->ctxt!=NULL) {
00171     serr->printf("StewartPlatform connection was dropped\n");
00172     xmlParserCtxtPtr tmp=(xmlParserCtxtPtr)theOne->ctxt;
00173     theOne->ctxt=NULL; //do this first to prevent recursive errors
00174     if(xmlParseChunk(tmp,"",0,1)!=0)
00175       serr->printf("StewartPlatform communication encountered a parsing error in isConnected\n");
00176     tmp->sax = NULL;
00177     xmlFreeParserCtxt(tmp); 
00178   }
00179   return con;
00180 }
00181 
00182 void StewartPlatformBehavior::closeComm() {
00183   if(cmdsock!=NULL) {
00184     cmdsock->printf("</connection>\n");
00185     wireless->close(cmdsock); //cmdsock is daemon, so it will still be valid
00186     if(ctxt!=NULL) {
00187       xmlParserCtxtPtr tmp=(xmlParserCtxtPtr)ctxt;
00188       ctxt=NULL; //do this first to prevent any recursive errors
00189       tmp->sax = NULL;
00190       xmlFreeParserCtxt(tmp); 
00191     }
00192   } else {
00193     ASSERT(ctxt==NULL,"non null parsing context with closed connection?");
00194     if(cmdsock!=NULL)
00195       wireless->close(cmdsock); //cmdsock is daemon, so it will still be valid
00196   }
00197 }
00198 
00199 
00200 
00201 
00202 /*! @file
00203  * @brief Implements StewartPlatformBehavior, moves the legs in synchrony to emulate the capabilities of a stewart platform
00204  * @author tss (Creator)
00205  * 
00206  * $Author: ejt $
00207  * $Name: tekkotsu-4_0 $
00208  * $Revision: 1.3 $
00209  * $State: Exp $
00210  * $Date: 2007/05/21 20:51:20 $
00211  */
00212 

Tekkotsu v4.0
Generated Thu Nov 22 00:54:56 2007 by Doxygen 1.5.4