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
00014 BehaviorBase::DoStart();
00015
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
00025 erouter->removeListener(this);
00026
00027 wireless->setDaemon(cmdsock,false);
00028 closeComm();
00029 cmdsock=NULL;
00030
00031 BehaviorBase::DoStop();
00032 }
00033
00034 void StewartPlatformBehavior::setParam(const std::string& cmd, float value) {
00035
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 & ) {
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
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
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
00105
00106
00107
00108
00109
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
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 * , 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 * , 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 * , 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
00168
00169
00170 if(!con && theOne->ctxt!=NULL) {
00171 serr->printf("StewartPlatform connection was dropped\n");
00172 xmlParserCtxtPtr tmp=(xmlParserCtxtPtr)theOne->ctxt;
00173 theOne->ctxt=NULL;
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);
00186 if(ctxt!=NULL) {
00187 xmlParserCtxtPtr tmp=(xmlParserCtxtPtr)ctxt;
00188 ctxt=NULL;
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);
00196 }
00197 }
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212