roseus.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage, Inc. nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  */
34 
35 // author: Kei Okada
36 
37 
38 #include <stdio.h>
39 #include <stdlib.h>
40 #include <unistd.h>
41 #include <string.h>
42 #include <signal.h>
43 #include <math.h>
44 #include <time.h>
45 #include <pthread.h>
46 #include <setjmp.h>
47 #include <errno.h>
48 
49 #include <list>
50 #include <vector>
51 #include <set>
52 #include <string>
53 #include <map>
54 #include <sstream>
55 
56 #include <cstdio>
57 #include <boost/thread/mutex.hpp>
58 #include <boost/thread/condition.hpp>
59 #include <boost/shared_ptr.hpp>
60 #include <boost/algorithm/string.hpp>
61 #include <boost/variant.hpp>
62 #include <boost/foreach.hpp>
63 
64 #include <ros/init.h>
65 #include <ros/time.h>
66 #include <ros/rate.h>
67 #include <ros/master.h>
68 #include <ros/this_node.h>
69 #include <ros/node_handle.h>
70 #include <ros/service.h>
72 #include <ros/service_manager.h>
73 #include <ros/connection.h>
74 #include <rospack/rospack.h>
75 #include <ros/param.h>
76 #include <ros/callback_queue.h>
77 
78 // for eus.h
79 #define class eus_class
80 #define throw eus_throw
81 #define export eus_export
82 #define vector eus_vector
83 #define string eus_string
84 
85 #include "eus.h"
86 extern "C" {
87 #ifdef ROSPACK_EXPORT
89 #else
91 #endif
92  pointer ___roseus(register context *ctx, int n, pointer *argv, pointer env);
94  char modname[] = "___roseus";
95  return add_module_initializer(modname, (pointer (*)())___roseus);}
96  /* get_string is originally defined in lisp/c/makes.c, but it is also defined in Python.so linked from rospack.so
97  to avoid confusion, we have to explictly re-define this method, specially for OSX */
98  byte *get_string(register pointer s){
99  if (isstring(s)) return(s->c.str.chars);
100  if (issymbol(s)) return(s->c.sym.pname->c.str.chars);
101  else error(E_NOSTRING); return NULL; }
102 }
103 
104 #undef class
105 #undef throw
106 #undef export
107 #undef vector
108 #undef string
109 
110 namespace ros {
111  namespace master {
113  void init(const M_string& remappings);
114  }
115  namespace param {
116  void init(const M_string& remappings);
117  }
118 }
119 
120 using namespace ros;
121 using namespace std;
122 
123 #define isInstalledCheck \
124  if( ! ros::ok() ) { error(E_USER,"You must call (ros::roseus \"name\") before creating the first NodeHandle"); }
125 
127 {
128 public:
131  }
134  map<string, boost::shared_ptr<Publisher> > mapAdvertised;
135  map<string, boost::shared_ptr<Subscriber> > mapSubscribed;
136  map<string, boost::shared_ptr<ServiceServer> > mapServiced;
137  map<string, Timer > mapTimered;
138 
139  map<string, boost::shared_ptr<NodeHandle> > mapHandle;
140 };
141 
143 static bool s_bInstalled = false;
144 #define s_node s_staticdata.node
145 #define s_rate s_staticdata.rate
146 #define s_mapAdvertised s_staticdata.mapAdvertised
147 #define s_mapSubscribed s_staticdata.mapSubscribed
148 #define s_mapServiced s_staticdata.mapServiced
149 #define s_mapTimered s_staticdata.mapTimered
150 #define s_mapHandle s_staticdata.mapHandle
151 
153 extern pointer LAMCLOSURE;
154 
155 /***********************************************************
156  * Message wrapper
157  ************************************************************/
158 
159 string getString(pointer message, pointer method) {
160  context *ctx = current_ctx;
161  pointer r, curclass;
162  if ((pointer)findmethod(ctx,method,classof(message),&curclass)!=NIL) {
163  r = csend(ctx,message,method,0);
164  } else if ((pointer)findmethod(ctx,K_ROSEUS_GET,classof(message),&curclass) != NIL ) {
165  r = csend(ctx,message,K_ROSEUS_GET,1,method);
166  } else {
167  r = NULL;
168 #ifdef x86_64
169  ROS_ERROR("could not find method %s for pointer %lx",
170  get_string(method), (long unsigned int)message);
171 #else
172  ROS_ERROR("could not find method %s for pointer %x",
173  get_string(method), (unsigned int)message);
174 #endif
175  }
176  if ( !isstring(r) ) {
177  pointer dest=(pointer)mkstream(ctx,K_OUT,makebuffer(64));
178  prinx(ctx,message,dest);
179  pointer str = makestring((char *)dest->c.stream.buffer->c.str.chars,
180  intval(dest->c.stream.count));
181  ROS_ERROR("send %s to %s returns nil", get_string(method), get_string(str));
182  }
183  ROS_ASSERT(isstring(r));
184  string ret = (char *)get_string(r);
185  return ret;
186 }
187 
188 int getInteger(pointer message, pointer method) {
189  context *ctx = current_ctx;
190  pointer a,curclass;
191  vpush(message);
192  a = (pointer)findmethod(ctx,method,classof(message),&curclass);
193  if (a!=NIL) {
194  pointer r = csend(ctx,message,method,0);
195  vpop(); // message
196  return (ckintval(r));
197  } else {
198 #ifdef x86_64
199  ROS_ERROR("could not find method %s for pointer %lx",
200  get_string(method), (long unsigned int)message);
201 #else
202  ROS_ERROR("could not find method %s for pointer %x",
203  get_string(method), (unsigned int)message);
204 #endif
205  vpop(); // message
206  }
207  return 0;
208 }
209 
211 {
212 public:
215 
216  EuslispMessage(pointer message) : _message(message) {
217  }
219  context *ctx = current_ctx;
220  if (ctx!=euscontexts[0])ROS_WARN("ctx is not correct %d\n",thr_self());
221  if ( isclass(r._message) ) {
222  //ROS_ASSERT(isclass(r._message));
223  vpush(r._message);
224  _message = makeobject(r._message);
225  vpush(_message);
226  csend(ctx,_message,K_ROSEUS_INIT,0);
227  vpop(); // _message
228  vpop(); // r._message
229  } else {
230  ROS_WARN("r._message must be class");prinx(ctx,r._message,ERROUT);flushstream(ERROUT);terpri(ERROUT);
231  _message = r._message;
232  }
233  }
234  virtual ~EuslispMessage() { }
235 
236  virtual void replaceContents (pointer newMessage) {
237  _message = newMessage;
238  }
239 
240  virtual const string __getDataType() const {
241  return getString(_message, K_ROSEUS_DATATYPE);
242  }
243  virtual const string __getMD5Sum() const {
244  return getString(_message, K_ROSEUS_MD5SUM);
245  }
246  virtual const string __getMessageDefinition() const {
247  return getString(_message, K_ROSEUS_DEFINITION);
248  }
249  virtual const string __getServiceDataType() const {
250  return getString(_message, K_ROSEUS_DATATYPE);
251  }
252  virtual const string __getServerMD5Sum() const {
253  return getString(_message, K_ROSEUS_MD5SUM);
254  }
255 
256  uint32_t serializationLength() const {
257  context *ctx = current_ctx;
258  if (ctx!=euscontexts[0])ROS_WARN("ctx is not correct %d\n",thr_self());
259  pointer a,curclass;
260  a = (pointer)findmethod(ctx,K_ROSEUS_SERIALIZATION_LENGTH,classof(_message),&curclass);
261  ROS_ASSERT(a!=NIL);
262  return getInteger(_message, K_ROSEUS_SERIALIZATION_LENGTH);
263  }
264 
265  virtual uint8_t *serialize(uint8_t *writePtr, uint32_t seqid) const
266  {
267  context *ctx = current_ctx;
268  if (ctx!=euscontexts[0])ROS_WARN("ctx is not correct %d\n",thr_self());
269  pointer a,curclass;
270  vpush(_message); // to avoid GC
271  uint32_t len = serializationLength();
272  vpop(); // pop _message
273  a = (pointer)findmethod(ctx,K_ROSEUS_SERIALIZE,classof(_message),&curclass);
274  ROS_ASSERT(a!=NIL);
275  pointer r = csend(ctx,_message,K_ROSEUS_SERIALIZE,0);
276  ROS_ASSERT(isstring(r));
277  memcpy(writePtr, get_string(r), len);
278  //ROS_INFO("serialize");
279 
280  return writePtr + len;
281  }
282 
283  virtual uint8_t *deserialize(uint8_t *readPtr, uint32_t sz)
284  {
285  context *ctx = current_ctx;
286  if (ctx!=euscontexts[0])ROS_WARN("ctx is not correct %d\n",thr_self());
287  pointer a,curclass;
288 
289  if ( sz == 0 ) {
290  ROS_DEBUG("empty message!");
291  return readPtr;
292  }
293  vpush(_message);
294  a = (pointer)findmethod(ctx,K_ROSEUS_DESERIALIZE,classof(_message),&curclass);
295  ROS_ASSERT(a!=NIL);
296  pointer p = makestring((char *)readPtr, sz);
297  pointer r;
298  r = csend(ctx,_message,K_ROSEUS_DESERIALIZE,1,p);
299  ROS_ASSERT(r!=NIL);
300  //ROS_INFO("deserialize %d", __serialized_length);
301  vpop(); // pop _message
302 
303  return readPtr + sz;
304  }
305 };
306 
308  if ( eus_msg->_connection_header == NULL ||
309  eus_msg->_connection_header->size() == 0 ) {
310  return;
311  }
312  context *ctx = current_ctx;
313  // store conection headers
314  register pointer ret, header;
315  ret = cons(ctx, NIL, NIL);
316  header = ret;
317  vpush(ret);
318  for(map<string, string>::iterator it = eus_msg->_connection_header->begin(); it != eus_msg->_connection_header->end(); it++){
319  ccdr(ret) = cons(ctx,cons(ctx,makestring((char *)it->first.c_str(), it->first.length()),
320  makestring((char *)it->second.c_str(), it->second.length())),NIL);
321  ret = ccdr(ret);
322  }
323  /* (setslot obj class index newval) */
324  pointer slot_args[4] = {eus_msg->_message, classof(eus_msg->_message), K_ROSEUS_CONNECTION_HEADER, ccdr(header)};
325  SETSLOT(ctx, 4, slot_args);
326  vpop();
327 }
328 
329 namespace ros{
330  namespace serialization{
331 template<> struct Serializer<EuslispMessage> {
332  template<typename Stream>
333  inline static void write(Stream& stream, boost::call_traits<EuslispMessage>::param_type t) {
334  t.serialize(stream.getData(), 0);
335  }
336 
337  template<typename Stream>
338  inline static void read(Stream& stream, boost::call_traits<EuslispMessage>::reference t) {
339  t.deserialize(stream.getData(), stream.getLength());
340  }
341  inline static uint32_t serializedLength(boost::call_traits<EuslispMessage>::param_type t) {
342  return t.serializationLength();
343  }
344 };
345  }
346 }
347 
348 /************************************************************
349  * Subscriptions
350  ************************************************************/
352 public:
353  pointer _scb,_args;
354  EuslispMessage _msg;
355 
357  context *ctx = current_ctx;
358  //ROS_WARN("func");prinx(ctx,scb,ERROUT);flushstream(ERROUT);terpri(ERROUT);
359  //ROS_WARN("argc");prinx(ctx,args,ERROUT);flushstream(ERROUT);terpri(ERROUT);
360  if (piscode(scb)) { // compiled code
361  _scb=scb;
362  } else if ((ccar(scb))==LAMCLOSURE) { // uncompiled code
363  if ( ccar(ccdr(scb)) != NIL ) { // function
364  _scb=ccar(ccdr(scb));
365  } else { // lambda function
366  _scb=scb;
367  }
368  } else {
369  ROS_ERROR("subscription callback function install error");
370  }
371  // avoid gc
372  pointer p=gensym(ctx);
373  setval(ctx,intern(ctx,(char*)(p->c.sym.pname->c.str.chars),strlen((char*)(p->c.sym.pname->c.str.chars)),lisppkg),cons(ctx,scb,args));
374  }
376  ROS_ERROR("subscription gc");
377  }
379 #if DEBUG
380  cerr << __PRETTY_FUNCTION__ << endl;
381  cerr << "param.length = " << param.length << endl;
382  cerr << "param.buffer = " << (param.buffer + 4) << endl;
383  cerr << "c_header == " << param.connection_header << endl;
384  for(map<string, string>::iterator it = param.connection_header->begin(); it != param.connection_header->end(); it++){
385  cerr << " " << it->first << " : " << it->second << endl;
386  }
387 #endif
388  ros::VoidConstPtr ptr(new EuslispMessage(_msg));
389  EuslispMessage *eus_msg = (EuslispMessage *)(ptr.get());
390  eus_msg->deserialize(param.buffer, param.length);
391 
392  eus_msg->_connection_header = param.connection_header;
393  return ptr;
394  }
395  virtual void call(ros::SubscriptionCallbackHelperCallParams& param) {
396  EuslispMessage* eus_msg = (EuslispMessage *)((void *)param.event.getConstMessage().get());
397  context *ctx = current_ctx;
398  pointer argp=_args;
399  int argc=0;
400  //ROS_WARN("func");prinx(ctx,_scb,ERROUT);flushstream(ERROUT);terpri(ERROUT);
401  //ROS_WARN("argc");prinx(ctx,argp,ERROUT);flushstream(ERROUT);terpri(ERROUT);
402  vpush(eus_msg->_message); // to avoid GC
403  if ( ! ( issymbol(_scb) || piscode(_scb) || ccar(_scb)==LAMCLOSURE ) ) {
404  ROS_ERROR("%s : can't find callback function", __PRETTY_FUNCTION__);
405  }
406 
407  // store connection header
408  StoreConnectionHeader(eus_msg);
409 
410  while(argp!=NIL){ ckpush(ccar(argp)); argp=ccdr(argp); argc++;}
411  vpush((pointer)(eus_msg->_message));argc++;
412 
413  ufuncall(ctx,(ctx->callfp?ctx->callfp->form:NIL),_scb,(pointer)(ctx->vsp-argc),NULL,argc);
414  while(argc-->0)vpop();
415  vpop(); // remove eus_msg._message
416  }
417  virtual const std::type_info& getTypeInfo() {
418  return typeid(EuslispMessage);
419  }
420  virtual bool isConst(){
421  return true;
422  }
423  virtual bool hasHeader(){
424  return true;
425  }
426 };
427 
428 /************************************************************
429  * ServiceCall
430  ************************************************************/
432 public:
433  pointer _scb, _args;
434  EuslispMessage _req, _res;
435  string md5, datatype, requestDataType, responseDataType,
436  requestMessageDefinition, responseMessageDefinition;
437 
438  EuslispServiceCallbackHelper(pointer scb, pointer args, string smd5, string sdatatype, pointer reqclass, pointer resclass) : _args(args), _req(reqclass), _res(resclass), md5(smd5), datatype(sdatatype) {
439  context *ctx = current_ctx;
440  //ROS_WARN("func");prinx(ctx,scb,ERROUT);flushstream(ERROUT);terpri(ERROUT);
441  //ROS_WARN("argc");prinx(ctx,args,ERROUT);flushstream(ERROUT);terpri(ERROUT);
442 
443  if (piscode(scb)) { // compiled code
444  _scb=scb;
445  } else if ((ccar(scb))==LAMCLOSURE) {
446  if ( ccar(ccdr(scb)) != NIL ) { // function
447  _scb=ccar(ccdr(scb));
448  } else { // lambda function
449  _scb=scb;
450  }
451  } else {
452  ROS_ERROR("service callback function install error");
453  }
454  // avoid gc
455  pointer p=gensym(ctx);
456  setval(ctx,intern(ctx,(char*)(p->c.sym.pname->c.str.chars),strlen((char*)(p->c.sym.pname->c.str.chars)),lisppkg),cons(ctx,scb,args));
457 
458  requestDataType = _req.__getDataType();
459  responseDataType = _res.__getDataType();
460  requestMessageDefinition = _req.__getMessageDefinition();
461  responseMessageDefinition = _res.__getMessageDefinition();
462  }
464 
466  virtual boost::shared_ptr<EuslispMessage> createResponse() { return boost::shared_ptr<EuslispMessage>(new EuslispMessage(_res)); }
467 
468  virtual std::string getMD5Sum() { return md5; }
469  virtual std::string getDataType() { return datatype; }
470  virtual std::string getRequestDataType() { return requestDataType; }
471  virtual std::string getResponseDataType() { return responseDataType; }
472  virtual std::string getRequestMessageDefinition() { return requestMessageDefinition; }
473  virtual std::string getResponseMessageDefinition() { return responseMessageDefinition; }
474 
476 #if DEBUG
477  cerr << __PRETTY_FUNCTION__ << endl;
478  cerr << "param.length = " << params.request.num_bytes << endl;
479  cerr << "param.buffer = " << (params.request.message_start + 4) << endl;
480  cerr << "c_header == " << params.connection_header << endl;
481  for(map<string, string>::iterator it = params.connection_header->begin(); it != params.connection_header->end(); it++){
482  cerr << " " << it->first << " : " << it->second << endl;
483  }
484 #endif
485  context *ctx = current_ctx;
486  pointer r, argp=_args;
487  int argc=0;
488 
489  vpush(_res._message); // _res._message
490  vpush(_req._message); // _res._message, _req._message
491 
492  if ( ! ( issymbol(_scb) || piscode(_scb) || ccar(_scb)==LAMCLOSURE ) ) {
493  ROS_ERROR("%s : can't find callback function", __PRETTY_FUNCTION__);
494  }
495  // Deserialization
496  EuslispMessage eus_msg(_req);
497  vpush(eus_msg._message); // _res._message, _req._message, eus_msg._message
498  eus_msg.deserialize(params.request.message_start, params.request.num_bytes);
499 
500  // store connection header
501  eus_msg._connection_header = params.connection_header;
502  StoreConnectionHeader(&eus_msg);
503 
504  while(argp!=NIL){ ckpush(ccar(argp)); argp=ccdr(argp); argc++;}
505  vpush((pointer) eus_msg._message);argc++;
506 
507  r = ufuncall(ctx, (ctx->callfp?ctx->callfp->form:NIL),
508  _scb, (pointer)(ctx->vsp-argc),
509  NULL, argc);
510  while(argc-->0)vpop();// _res._message, _req._message, eus_msg._message
511  vpush(r); // _res._message, _req._message, eus_msg._message, r,
512  // Serializaion
513  EuslispMessage eus_res(_res);
514  eus_res.replaceContents(r);
515  // check return value is valid
516  pointer ret_serialize_method, ret_class;
517  if (ispointer(r)) {
518  ret_serialize_method = (pointer)findmethod(ctx,K_ROSEUS_SERIALIZATION_LENGTH,classof(r),&ret_class); }
519  if (!ispointer(r) || ret_serialize_method == NIL) {
520  ROS_ERROR("you may not return valid value in service callback");
521  vpop(); // _res._message, _req._message, eus_msg._message, r
522  vpop(); // _res._message, _req._message, eus_msg._message
523  vpop(); // _res._message, _req._message,
524  vpop(); // _res._message
525  return false;
526  }
527  vpush(eus_res._message); // _res._message, _req._message, eus_msg._message, r, eus_res._message
528  uint32_t serialized_length = eus_res.serializationLength();
529  params.response.num_bytes = serialized_length + 5; // add 5 bytes of message header
530  params.response.buf.reset (new uint8_t[params.response.num_bytes]);
531  params.response.message_start = 0;
532 
533  // SerializedResponseMessage
534  uint8_t *tmp = params.response.buf.get();
535  *tmp++ = 1; // 1 byte of success services flag, now always set true
536  *tmp++ = (uint8_t)((serialized_length >> 0) & 0xFF); // 4bytes of message length
537  *tmp++ = (uint8_t)((serialized_length >> 8) & 0xFF);
538  *tmp++ = (uint8_t)((serialized_length >> 16) & 0xFF);
539  *tmp++ = (uint8_t)((serialized_length >> 24) & 0xFF);
540  eus_res.serialize(tmp, 0);
541  // store connection header
542  eus_res._connection_header = params.connection_header;
543  StoreConnectionHeader(&eus_res);
544 #if DEBUG
545  cerr << "num bytes = " << params.response.num_bytes << endl;
546  ROS_INFO("message_start = %X",params.response.message_start);
547  ROS_INFO("message_ptr = %X",params.response.buf.get());
548  tmp = params.response.buf.get();
549  for(int i=0;i<params.response.num_bytes;i++){
550  ROS_INFO("%X", tmp[i]);
551  }
552 #endif
553  vpop(); // _res._message, _req._message, eus_msg._message, r, eus_res._message
554  vpop(); // _res._message, _req._message, eus_msg._message, r
555  vpop(); // _res._message, _req._message, eus_msg._message
556  vpop(); // _res._message, _req._message,
557  vpop(); // _res._message
558  return true;
559  }
560 };
561 
562 void roseusSignalHandler(int sig)
563 {
564  // memoize for euslisp handler...
565  context *ctx=euscontexts[thr_self()];
566  ctx->intsig = sig;
567 }
568 
569 /************************************************************
570  * EUSLISP functions
571  ************************************************************/
572 pointer ROSEUS(register context *ctx,int n,pointer *argv)
573 {
574  char name[256] = "";
575  uint32_t options = 0;
576  int cargc = 0;
577  char *cargv[32];
578 
579  if( s_bInstalled ) {
580  ROS_WARN("ROSEUS is already installed as %s", ros::this_node::getName().c_str());
581  return (T);
582  }
583 
584  ckarg(3);
585  if (isstring(argv[0]))
586  strncpy(name,(char *)(argv[0]->c.str.chars),255);
587  else error(E_NOSTRING);
588  options = ckintval(argv[1]);
589  pointer p = argv[2];
590  if (islist(p)) {
591  while (1) {
592  if (!iscons(p)) break;
593  cargv[cargc]=(char *)((ccar(p))->c.str.chars);
594  cargc++;
595  p=ccdr(p);
596  }
597  } else error(E_NOSEQ);
598 
599  // convert invalid node name charactors to _, we assume it is '-'
600  for (unsigned int i=0; i < strlen(name); i++)
601  if ( ! (isalpha(name[i]) || isdigit(name[i])) ) name[i] = '_';
602 
603  K_ROSEUS_MD5SUM = defkeyword(ctx,"MD5SUM-");
604  K_ROSEUS_DATATYPE = defkeyword(ctx,"DATATYPE-");
605  K_ROSEUS_DEFINITION = defkeyword(ctx,"DEFINITION-");
606  K_ROSEUS_CONNECTION_HEADER = intern(ctx,"_CONNECTION-HEADER",18,findpkg(makestring("ROS",3)));
607  K_ROSEUS_SERIALIZATION_LENGTH = defkeyword(ctx,"SERIALIZATION-LENGTH");
608  K_ROSEUS_SERIALIZE = defkeyword(ctx,"SERIALIZE");
609  K_ROSEUS_DESERIALIZE = defkeyword(ctx,"DESERIALIZE");
610  K_ROSEUS_GET = defkeyword(ctx,"GET");
611  K_ROSEUS_INIT = defkeyword(ctx,"INIT");
612  K_ROSEUS_REQUEST = defkeyword(ctx,"REQUEST");
613  K_ROSEUS_RESPONSE = defkeyword(ctx,"RESPONSE");
614  K_ROSEUS_GROUPNAME = defkeyword(ctx,"GROUPNAME");
615  K_ROSEUS_ONESHOT = defkeyword(ctx,"ONESHOT");
616  K_ROSEUS_LAST_EXPECTED = defkeyword(ctx,"LAST-EXPECTED");
617  K_ROSEUS_LAST_REAL = defkeyword(ctx,"LAST-REAL");
618  K_ROSEUS_CURRENT_EXPECTED = defkeyword(ctx,"CURRENT-EXPECTED");
619  K_ROSEUS_CURRENT_REAL = defkeyword(ctx,"CURRENT-REAL");
620  K_ROSEUS_LAST_DURATION = defkeyword(ctx,"LAST-DURATION");
621  K_ROSEUS_SEC = defkeyword(ctx,"SEC");
622  K_ROSEUS_NSEC = defkeyword(ctx,"NSEC");
623 
624  s_mapAdvertised.clear();
625  s_mapSubscribed.clear();
626  s_mapServiced.clear();
627  s_mapTimered.clear();
628  s_mapHandle.clear();
629 
630  /*
631  set locale to none to let C RTL assume logging string can contain non-ascii characters.
632  Refer: https://logging.apache.org/log4cxx/latest_stable/faq.html
633  */
634  setlocale(LC_ALL, "");
635 
636  /*
637  force to flag ros::init_options::NoSigintHandler.
638  In fact, this code make no sence, because we steals
639  SIGINT handler by the following `signal'.
640  */
642 
643  /*
644  clear ros::master::g_uri which is defined in ros::master::init in __roseus.
645  this occurs if user set unix::setenv("ROS_MASTER_URI") between __roseus and
646  ROSEUS.
647  */
648  if (!ros::master::g_uri.empty()) {
649  if ( ros::master::g_uri != getenv("ROS_MASTER_URI") ) {
650  ROS_WARN("ROS master uri will be changed!!, master uri %s, which is defineed previously is differ from current ROS_MASTE_URI(%s)", ros::master::g_uri.c_str(), getenv("ROS_MASTER_URI"));
651  ros::master::g_uri.clear();
652  }
653  }
654  try {
655  ros::init(cargc, cargv, name, options);
656  } catch (const ros::InvalidNameException &e) {
657  ROS_ERROR("%s",e.what());
659  return(NIL);
660  }
661 
662  s_node.reset(new ros::NodeHandle());
663  s_rate.reset(new ros::Rate(50));
664 
665  s_bInstalled = true;
666 
667  // install signal handler for sigint. DO NOT call unix:signal after
668  // ros::roseus
669  signal(SIGINT, roseusSignalHandler);
670  return (T);
671 }
672 
674 {
676  string groupname;
677  string ns;
678 
679  ckarg2(1, 2);
680  // ;; arguments ;;
681  // groupname [ namespace ]
682 
683  if (isstring(argv[0])) groupname.assign((char *)get_string(argv[0]));
684  else error(E_NOSTRING);
685  if ( n > 1 ) {
686  if(isstring(argv[1])) ns.assign((char *)get_string(argv[1]));
687  else error(E_NOSTRING);
688  }
689 
690  if( s_mapHandle.find(groupname) != s_mapHandle.end() ) {
691  ROS_DEBUG("groupname %s is already used", groupname.c_str());
692  return (NIL);
693  }
694 
696  if ( n > 1 ) {
698  s_mapHandle[groupname] = hd;
699  } else {
701  s_mapHandle[groupname] = hd;
702  }
703  //add callbackqueue to hd
704  hd->setCallbackQueue( new CallbackQueue() );
705 
706  return (T);
707 }
708 
710 {
712  while (ctx->intsig==0 && ros::ok()) {
713  ros::spinOnce();
714  s_rate->sleep();
715  }
716  return (NIL);
717 }
718 
720 {
722  ckarg2(0, 1);
723  // ;; arguments ;;
724  // [ groupname ]
725 
726  if ( n > 0 ) {
727  string groupname;
728  if (isstring(argv[0])) groupname.assign((char *)get_string(argv[0]));
729  else error(E_NOSTRING);
730 
731  map<string, boost::shared_ptr<NodeHandle > >::iterator it = s_mapHandle.find(groupname);
732  if( it == s_mapHandle.end() ) {
733  ROS_ERROR("Groupname %s is missing", groupname.c_str());
734  return (T);
735  }
736  boost::shared_ptr<NodeHandle > hdl = (it->second);
737  // spin this nodehandle
738  ((CallbackQueue *)hdl->getCallbackQueue())->callAvailable();
739 
740  return (NIL);
741  } else {
742  ros::spinOnce();
743  return (NIL);
744  }
745 }
746 
748 {
750  pointer timevec;
752 
753  timevec=makevector(C_INTVECTOR,2);
754  vpush(timevec);
755  timevec->c.ivec.iv[0]=t.sec;
756  timevec->c.ivec.iv[1]=t.nsec;
757  vpop();
758  return (timevec);
759 }
760 
762 {
764  numunion nu;
765  ckarg(1);
766  float timeout=ckfltval(argv[0]);
767  s_rate.reset(new ros::Rate(timeout));
768  return(T);
769 }
770 
772 {
774  s_rate->sleep();
775  return (T);
776 }
777 
779 {
781  numunion nu;
782  ckarg(1);
783  float sleep=ckfltval(argv[0]);
784  ros::Duration(sleep).sleep();
785  return(T);
786 }
787 
788 pointer ROSEUS_OK(register context *ctx,int n,pointer *argv)
789 {
790  if (ros::ok()) {
791  return (T);
792  } else {
793  return (NIL);
794  }
795 }
796 
797 
798 #define def_rosconsole_formatter(funcname, rosfuncname) \
799  pointer funcname(register context *ctx,int n,pointer *argv) \
800  { pointer *argv2,msg; \
801  int argc2; \
802  argc2 = n+1; \
803  argv2 = (pointer *)malloc(sizeof(pointer)*argc2); \
804  argv2[0] = NIL; \
805  for(int i=0;i<n;i++) argv2[i+1]=argv[i] ; \
806  msg = XFORMAT(ctx, argc2, argv2); \
807  rosfuncname("%s", msg->c.str.chars); \
808  free(argv2); \
809  return (T); \
810  }
811 
812 def_rosconsole_formatter(ROSEUS_ROSDEBUG, ROS_DEBUG)
813 def_rosconsole_formatter(ROSEUS_ROSINFO, ROS_INFO)
814 def_rosconsole_formatter(ROSEUS_ROSWARN, ROS_WARN)
815 def_rosconsole_formatter(ROSEUS_ROSERROR, ROS_ERROR)
816 def_rosconsole_formatter(ROSEUS_ROSFATAL, ROS_FATAL)
817 
818 pointer ROSEUS_EXIT(register context *ctx,int n,pointer *argv)
819 {
820  ROS_INFO("%s", __PRETTY_FUNCTION__);
821  if( s_bInstalled ) {
822  ROS_INFO("exiting roseus %ld", (n==0)?n:ckintval(argv[0]));
823  s_mapAdvertised.clear();
824  s_mapSubscribed.clear();
825  s_mapServiced.clear();
826  s_mapTimered.clear();
827  s_mapHandle.clear();
828  ros::shutdown();
829  }
830  if (n==0) _exit(0);
831  else _exit(ckintval(argv[0]));
832 }
833 
834 /************************************************************
835  * ROSEUS Publisher/Subscriber
836  ************************************************************/
838 {
840  string topicname;
841  pointer message, fncallback, args;
842  int queuesize = 1;
843  NodeHandle *lnode = s_node.get();
844 
845  // ;; arguments ;;
846  // topicname message_type callbackfunc args0 ... argsN [ queuesize ] [ :groupname groupname ]
847  if (isstring(argv[0])) topicname = ros::names::resolve((char *)get_string(argv[0]));
848  else error(E_NOSTRING);
849 
850  if (n > 1 && issymbol(argv[n-2]) && isstring(argv[n-1])) {
851  if (argv[n-2] == K_ROSEUS_GROUPNAME) {
852  string groupname;
853  groupname.assign((char *)get_string(argv[n-1]));
854  map<string, boost::shared_ptr<NodeHandle > >::iterator it = s_mapHandle.find(groupname);
855  if( it != s_mapHandle.end() ) {
856  ROS_DEBUG("subscribe with groupname=%s", groupname.c_str());
857  lnode = (it->second).get();
858  } else {
859  ROS_ERROR("Groupname %s is missing. Topic %s is not subscribed. Call (ros::create-nodehandle \"%s\") first.",
860  groupname.c_str(), topicname.c_str(), groupname.c_str());
861  return (NIL);
862  }
863  n -= 2;
864  }
865  }
866  if (isint(argv[n-1])) {queuesize = ckintval(argv[n-1]);n--;}
867  ROS_DEBUG("subscribe %s queuesize=%d", topicname.c_str(), queuesize);
868  // TODO:need error checking
869  message = argv[1];
870  fncallback = argv[2];
871  args=NIL;
872  for (int i=n-1;i>=3;i--) args=cons(ctx,argv[i],args);
873 
874  EuslispMessage msg(message);
877  (new EuslispSubscriptionCallbackHelper(fncallback, args, message)));
878  SubscribeOptions so(topicname, queuesize, msg.__getMD5Sum(), msg.__getDataType());
879  so.helper = *callback;
880  Subscriber subscriber = lnode->subscribe(so);
882  if ( !!sub ) {
883  s_mapSubscribed[topicname] = sub;
884  } else {
885  ROS_ERROR("s_mapSubscribed");
886  }
887 
888  return (T);
889 }
890 
892 {
893  string topicname;
894 
895  ckarg(1);
896  if (isstring(argv[0])) topicname = ros::names::resolve((char *)get_string(argv[0]));
897  else error(E_NOSTRING);
898 
899  bool bSuccess = s_mapSubscribed.erase(topicname)>0;
900 
901  return (bSuccess?T:NIL);
902 }
903 
905 {
906  string topicname;
907  int ret;
908 
909  ckarg(1);
910  if (isstring(argv[0])) topicname = ros::names::resolve((char *)get_string(argv[0]));
911  else error(E_NOSTRING);
912 
913  bool bSuccess = false;
914  map<string, boost::shared_ptr<Subscriber> >::iterator it = s_mapSubscribed.find(topicname);
915  if( it != s_mapSubscribed.end() ) {
916  boost::shared_ptr<Subscriber> subscriber = (it->second);
917  ret = subscriber->getNumPublishers();
918  bSuccess = true;
919  }
920 
921  if ( ! bSuccess ) {
922  ROS_ERROR("attempted to getNumPublishers to topic %s, which was not " \
923  "previously subscribed. call (ros::subscribe \"%s\") first.",
924  topicname.c_str(), topicname.c_str());
925  }
926 
927  return (bSuccess?(makeint(ret)):NIL);
928 }
929 
931 {
932  string topicname;
933  string ret;
934 
935  ckarg(1);
936  if (isstring(argv[0])) topicname = ros::names::resolve((char *)get_string(argv[0]));
937  else error(E_NOSTRING);
938 
939  bool bSuccess = false;
940  map<string, boost::shared_ptr<Subscriber> >::iterator it = s_mapSubscribed.find(topicname);
941  if( it != s_mapSubscribed.end() ) {
942  boost::shared_ptr<Subscriber> subscriber = (it->second);
943  ret = subscriber->getTopic();
944  bSuccess = true;
945  }
946 
947  return (bSuccess?(makestring((char *)ret.c_str(), ret.length())):NIL);
948 }
949 
951 {
953  string topicname;
954  pointer message;
955  int queuesize = 1;
956  bool latch = false;
957 
958  ckarg2(2,4);
959  if (isstring(argv[0])) topicname = ros::names::resolve((char *)get_string(argv[0]));
960  else error(E_NOSTRING);
961 
962  message = argv[1];
963  if ( n > 2 ) {
964  queuesize = ckintval(argv[2]);
965  }
966  if ( n > 3 ) {
967  latch = (argv[3]!=NIL ? true : false);
968  }
969  ROS_DEBUG("advertise %s %d %d", topicname.c_str(), queuesize, latch);
970  if( s_mapAdvertised.find(topicname) != s_mapAdvertised.end() ) {
971  ROS_WARN("topic %s already advertised", topicname.c_str());
972  return (NIL);
973  }
974 
975  EuslispMessage msg(message);
976  AdvertiseOptions ao(topicname, queuesize, msg.__getMD5Sum(), msg.__getDataType(), msg.__getMessageDefinition());
977  ao.latch = latch;
978  Publisher publisher = s_node->advertise(ao);
980  if ( !!pub ) {
981  s_mapAdvertised[topicname] = pub;
982  } else {
983  ROS_ERROR("s_mapAdvertised");
984  }
985 
986  return (T);
987 }
988 
990 {
991  string topicname;
992 
993  ckarg(1);
994  if (isstring(argv[0])) topicname = ros::names::resolve((char *)get_string(argv[0]));
995  else error(E_NOSTRING);
996 
997  bool bSuccess = s_mapAdvertised.erase(topicname)>0;
998 
999  return (bSuccess?T:NIL);
1000 }
1001 
1003 {
1005  string topicname;
1006  pointer emessage;
1007 
1008  ckarg(2);
1009  if (isstring(argv[0])) topicname = ros::names::resolve((char *)get_string(argv[0]));
1010  else error(E_NOSTRING);
1011 
1012  emessage = argv[1];
1013 
1014  bool bSuccess = false;
1015  map<string, boost::shared_ptr<Publisher> >::iterator it = s_mapAdvertised.find(topicname);
1016  if( it != s_mapAdvertised.end() ) {
1017  boost::shared_ptr<Publisher> publisher = (it->second);
1018  EuslispMessage message(emessage);
1019  publisher->publish(message);
1020  bSuccess = true;
1021  }
1022 
1023  if ( ! bSuccess ) {
1024  ROS_ERROR("attempted to publish to topic %s, which was not " \
1025  "previously advertised. call (ros::advertise \"%s\") first.",
1026  topicname.c_str(), topicname.c_str());
1027  }
1028 
1029  return (T);
1030 }
1031 
1033 {
1034  string topicname;
1035  int ret;
1036 
1037  ckarg(1);
1038  if (isstring(argv[0])) topicname = ros::names::resolve((char *)get_string(argv[0]));
1039  else error(E_NOSTRING);
1040 
1041  bool bSuccess = false;
1042  map<string, boost::shared_ptr<Publisher> >::iterator it = s_mapAdvertised.find(topicname);
1043  if( it != s_mapAdvertised.end() ) {
1044  boost::shared_ptr<Publisher> publisher = (it->second);
1045  ret = publisher->getNumSubscribers();
1046  bSuccess = true;
1047  }
1048 
1049  if ( ! bSuccess ) {
1050  ROS_ERROR("attempted to getNumSubscribers to topic %s, which was not " \
1051  "previously advertised. call (ros::advertise \"%s\") first.",
1052  topicname.c_str(), topicname.c_str());
1053  }
1054 
1055  return (bSuccess?(makeint(ret)):NIL);
1056 }
1057 
1059 {
1060  string topicname;
1061  string ret;
1062 
1063  ckarg(1);
1064  if (isstring(argv[0])) topicname = ros::names::resolve((char *)get_string(argv[0]));
1065  else error(E_NOSTRING);
1066 
1067  bool bSuccess = false;
1068  map<string, boost::shared_ptr<Publisher> >::iterator it = s_mapAdvertised.find(topicname);
1069  if( it != s_mapAdvertised.end() ) {
1070  boost::shared_ptr<Publisher> publisher = (it->second);
1071  ret = publisher->getTopic();
1072  bSuccess = true;
1073  }
1074 
1075  return (bSuccess?(makestring((char *)ret.c_str(), ret.length())):NIL);
1076 }
1077 
1078 /************************************************************
1079  * ROSEUS ServiceCall
1080  ************************************************************/
1082 {
1084  string service;
1085  numunion nu;
1086 
1087  ckarg2(1,2);
1088  if (isstring(argv[0])) service = ros::names::resolve((char *)get_string(argv[0]));
1089  else error(E_NOSTRING);
1090 
1091  float timeout = -1;
1092 
1093  if( n > 1 && argv[1] != NIL)
1094  timeout = ckfltval(argv[1]);
1095 
1096  bool bSuccess = service::waitForService(service, ros::Duration(timeout));
1097 
1098  return (bSuccess?T:NIL);
1099 }
1100 
1102 {
1104  string service;
1105 
1106  ckarg(1);
1107  if (isstring(argv[0])) service = ros::names::resolve((char *)get_string(argv[0]));
1108  else error(E_NOSTRING);
1109 
1110  bool bSuccess = service::exists(service, true);
1111 
1112  return (bSuccess?T:NIL);
1113 }
1114 
1116 {
1118  string service;
1119  pointer emessage;
1120  bool persist = false;
1121  ckarg2(2,3);
1122  if (isstring(argv[0])) service = ros::names::resolve((char *)get_string(argv[0]));
1123  else error(E_NOSTRING);
1124  emessage = argv[1];
1125  if ( n > 2 ) {
1126  persist = (argv[2] != NIL ? true : false);
1127  }
1128  static std::map<std::string, ros::ServiceServerLinkPtr> service_link_cache;
1129  static std::map<std::string, std::string> service_md5_cache;
1130  EuslispMessage request(emessage);
1131  vpush(request._message); // to avoid GC, it may not be required...
1133  vpush(response._message); // to avoid GC, its important
1134 
1135  // NOTE: To make use of connection header, ServiceServerLink is manually handled instead of just invoking ServiceClient.call.
1136  // ServiceClientOptions sco(service, request.__getMD5Sum(), persist, M_string());
1137  // client = s_node->serviceClient(sco);
1138  // bool bSuccess = client.call(request, response, request.__getMD5Sum());
1140  if (persist) {
1141  if ( service_link_cache.find(service) != service_link_cache.end() &&
1142  ( ! service_link_cache[service]->isValid() ||
1143  service_md5_cache[service] !=request.__getMD5Sum() )) {
1144  service_link_cache[service]->getConnection()->drop(ros::Connection::Destructing);
1145  service_link_cache[service].reset();
1146  service_link_cache.erase(service);
1147  service_md5_cache.erase(service);
1148  }
1149 
1150  if (service_link_cache.find(service) == service_link_cache.end()) {
1151  service_link_cache[service] = ros::ServiceManager::instance()->createServiceServerLink(service, persist, request.__getMD5Sum(), request.__getMD5Sum(), M_string());
1152  service_md5_cache[service] = request.__getMD5Sum();
1153  }
1154  link = service_link_cache[service];
1155  } else {
1156  link = ros::ServiceManager::instance()->createServiceServerLink(service, persist, request.__getMD5Sum(), request.__getMD5Sum(), M_string());
1157  }
1158 
1159  bool bSuccess = true;
1161  ros::SerializedMessage ser_resp;
1162  if (link && link->isValid()) {
1163  bSuccess = link->call(ser_req, ser_resp);
1164  if ( bSuccess ) {
1165  try {
1167  } catch (std::exception& e) {
1168  ROS_ERROR("Exception thrown while deserializing service call: %s", e.what());
1169  bSuccess = false;
1170  }
1171  boost::shared_ptr<map<string, string> >connection_header = link->getConnection()->getHeader().getValues();
1172 #if DEBUG
1173  cerr << __PRETTY_FUNCTION__ << endl;
1174  cerr << "c_header == " << connection_header << endl;
1175  for(map<string, string>::iterator it = connection_header->begin(); it != connection_header->end(); it++){
1176  cerr << " " << it->first << " : " << it->second << endl;
1177  }
1178 #endif
1179  response._connection_header = connection_header;
1181  link.reset();
1182  // If we're shutting down but the node haven't finished yet, wait until we do
1183  while (ros::isShuttingDown() && ros::ok()) {
1184  ros::WallDuration(0.001).sleep();
1185  }
1186  }
1187  } else {
1188  bSuccess = false;
1189  ROS_ERROR("Failed to establish connection to service server");
1190  }
1191  vpop(); // pop response._message
1192  vpop(); // pop request._message
1193  if ( ! bSuccess ) {
1194  ROS_ERROR("attempted to call service %s, but failed ",
1195  ros::names::resolve(service).c_str());
1196  }
1197 
1198  return (response._message);
1199 }
1200 
1202 {
1204  string service;
1205  pointer emessage;
1206  pointer fncallback, args;
1207  NodeHandle *lnode = s_node.get();
1208 
1209  if (isstring(argv[0])) service = ros::names::resolve((char *)get_string(argv[0]));
1210  else error(E_NOSTRING);
1211  emessage = argv[1];
1212  fncallback = argv[2];
1213 
1214  if (n >= 5 && issymbol(argv[n-2]) && isstring(argv[n-1])) {
1215  if (argv[n-2] == K_ROSEUS_GROUPNAME) {
1216  string groupname;
1217  groupname.assign((char *)get_string(argv[n-1]));
1218  map<string, boost::shared_ptr<NodeHandle > >::iterator it = s_mapHandle.find(groupname);
1219  if( it != s_mapHandle.end() ) {
1220  ROS_DEBUG("advertising service with groupname=%s", groupname.c_str());
1221  lnode = (it->second).get();
1222  } else {
1223  ROS_ERROR("Groupname \"%s\" is missing. Service %s is not advertised. Call (ros::create-nodehandle \"%s\") first.",
1224  groupname.c_str(), service.c_str(), groupname.c_str());
1225  return (NIL);
1226  }
1227  n -= 2;
1228  }
1229  }
1230 
1231  args=NIL;
1232  for (int i=n-1;i>=3;i--) args=cons(ctx,argv[i],args);
1233  if( s_mapServiced.find(service) != s_mapServiced.end() ) {
1234  ROS_INFO("service %s already advertised", service.c_str());
1235  return (NIL);
1236  }
1237 
1238  EuslispMessage message(emessage);
1239  vpush(message._message); // to avoid GC in csend
1240  pointer request(csend(ctx,emessage,K_ROSEUS_GET,1,K_ROSEUS_REQUEST));
1242  vpop(); // pop message._message
1245  (new EuslispServiceCallbackHelper(fncallback, args, message.__getMD5Sum(),
1246  message.__getDataType(), request, response)));
1248  aso.service.assign(service);
1249  aso.datatype = (*callback->get()).getDataType();
1250  aso.md5sum = (*callback->get()).getMD5Sum();
1251  aso.req_datatype = (*callback->get()).getRequestDataType();
1252  aso.res_datatype = (*callback->get()).getResponseDataType();
1253  aso.helper = *callback;
1254  ServiceServer server = lnode->advertiseService(aso);
1256  if ( !!ser ) {
1257  s_mapServiced[service] = ser;
1258  } else {
1259  ROS_ERROR("s_mapServiced");
1260  }
1261 
1262  return (T);
1263 }
1264 
1266 {
1267  string service;
1268 
1269  ckarg(1);
1270  if (isstring(argv[0])) service = ros::names::resolve((char *)get_string(argv[0]));
1271  else error(E_NOSTRING);
1272 
1273  ROS_DEBUG("unadvertise %s", service.c_str());
1274  bool bSuccess = s_mapServiced.erase(service)>0;
1275 
1276  return (bSuccess?T:NIL);
1277 }
1278 
1279 void EusValueToXmlRpc(register context *ctx, pointer argp, XmlRpc::XmlRpcValue& rpc_value)
1280 {
1281  numunion nu;
1282 
1283  if ( islist(argp) && islist(ccar(argp)) && !islist(ccar(ccar(argp))) ) { // alist
1284  pointer a;
1285  int j;
1286  // set keys
1287  std::ostringstream stringstream;
1288  stringstream << "<value><struct>";
1289  a = argp;
1290  while(islist(a)){
1291  pointer v = ccar(a);
1292  if ( iscons(v) ) { // is alist
1293  if ( issymbol(ccar(v)) ) {
1294  string skey = string((char *)get_string(ccar(v)->c.sym.pname));
1295  boost::algorithm::to_lower(skey);
1296  stringstream << "<member><name>" << skey << "</name><value><boolean>0</boolean></value></member>";
1297  }
1298  else if ( isstring(ccar(v)) ) {
1299  string skey = string((char *)get_string(ccar(v)));
1300  stringstream << "<member><name>" << skey << "</name><value><boolean>0</boolean></value></member>";
1301  }else{
1302  ROS_ERROR("ROSEUS_SET_PARAM: EusValueToXmlRpc: invalid param name; requires symbol or string");prinx(ctx,ccar(v),ERROUT);flushstream(ERROUT);terpri(ERROUT);
1303  }
1304  }else{
1305  ROS_ERROR("ROSEUS_SET_PARAM: EusValueToXmlRpc: assuming alist");prinx(ctx,argp,ERROUT);flushstream(ERROUT);terpri(ERROUT);
1306  }
1307  a=ccdr(a);
1308  }
1309  stringstream << "</struct></value>";
1310  j=0; rpc_value.fromXml(stringstream.str(), &j);
1311  // set values
1312  a = argp;
1313  while(islist(a)){
1314  pointer v = ccar(a);
1315  if ( iscons(v) ) {
1316  if ( issymbol(ccar(v)) ) {
1317  string skey = string((char *)get_string(ccar(v)->c.sym.pname));
1318  boost::algorithm::to_lower(skey);
1320  EusValueToXmlRpc(ctx, ccdr(v), p);
1321  rpc_value[skey] = p;
1322  }
1323  else if ( isstring(ccar(v)) ) {
1324  string skey = string((char *)get_string(ccar(v)));
1326  EusValueToXmlRpc(ctx, ccdr(v), p);
1327  rpc_value[skey] = p;
1328  }
1329  }
1330  a=ccdr(a);
1331  }
1332  } else if ( islist(argp) ) { // list
1333  pointer a;
1334  int i;
1335  // get size
1336  a = argp;
1337  i = 0; while ( islist(a) ) { a=ccdr(a); i++; }
1338  rpc_value.setSize(i);
1339  // fill items
1340  a = argp;
1341  i = 0;
1342  while ( islist(a) ) {
1344  EusValueToXmlRpc(ctx, ccar(a), p);
1345  rpc_value[i] = p;
1346  a=ccdr(a);
1347  i++;
1348  }
1349  } else if ( isstring(argp) ) {
1350  rpc_value = (char *)get_string(argp);
1351  } else if ( isint(argp) ) {
1352  rpc_value = (int)(intval(argp));
1353  } else if ( isflt(argp) ) {
1354  rpc_value = (double)(fltval(argp));
1355  } else if (argp == T) {
1356  rpc_value = XmlRpc::XmlRpcValue((bool)true);
1357  } else if (argp == NIL) {
1358  rpc_value = XmlRpc::XmlRpcValue((bool)false);
1359  } else if ( issymbol(argp) ) {
1360  string s((char *)get_string(argp->c.sym.pname));
1361  boost::algorithm::to_lower(s);
1362  rpc_value = s.c_str();
1363  } else {
1364  ROS_ERROR("EusValueToXmlRpc: unknown parameters");prinx(ctx,argp,ERROUT);flushstream(ERROUT);terpri(ERROUT);
1366  }
1367 }
1368 
1370 {
1371  string key;
1372  string s;
1373 
1374  ckarg(2);
1375  if (isstring(argv[0])) key.assign((char *)get_string(argv[0]));
1376  else error(E_NOSTRING);
1378  EusValueToXmlRpc(ctx, argv[1], param);
1379  ros::param::set(key,param);
1380 
1381  return (T);
1382 }
1383 
1385 {
1386  numunion nu;
1387  pointer ret, first;
1388 
1389  if ( rpc_value.getType() == XmlRpc::XmlRpcValue::TypeBoolean ){
1390  if ( rpc_value ) return T; else return NIL;
1391  }
1392  else if ( rpc_value.getType() == XmlRpc::XmlRpcValue::TypeDouble ){
1393  return makeflt((double)rpc_value);
1394  }
1395  else if ( rpc_value.getType() == XmlRpc::XmlRpcValue::TypeInt ){
1396  return makeint((int)rpc_value);
1397  }
1398  else if ( rpc_value.getType() == XmlRpc::XmlRpcValue::TypeString ){
1399  std::string str = rpc_value;
1400  return makestring((char*)str.c_str(), ((std::string)rpc_value).length());
1401  }
1402  else if ( rpc_value.getType() == XmlRpc::XmlRpcValue::TypeArray ){
1403  ret = cons(ctx, NIL, NIL);
1404  first = ret;
1405  vpush(ret);
1406  for ( int i = 0; i < rpc_value.size(); i++){
1407  ccdr(ret) = cons(ctx, XmlRpcToEusValue(ctx, rpc_value[i]), NIL);
1408  ret = ccdr(ret);
1409  }
1410  vpop(); // vpush(ret);
1411  return ccdr(first);
1412  }
1413  else if ( rpc_value.getType() == XmlRpc::XmlRpcValue::TypeStruct ){
1414  ret = cons(ctx, NIL, NIL);
1415  first = ret;
1416  vpush(ret);
1417  XmlRpc::XmlRpcValue::iterator it = rpc_value.begin();
1418  while(it !=rpc_value.end()) {
1419  std::string key = it->first;
1420  pointer tmp = cons(ctx, makestring((char*)key.c_str(), key.length()), NIL);
1421  vpush(tmp);
1422  ccdr(tmp) = XmlRpcToEusValue(ctx, it->second);
1423  ccdr(ret) = cons(ctx, tmp, NIL);
1424  ret = ccdr(ret);
1425  vpop(); // vpush(tmp);
1426  it++;
1427  }
1428  vpop(); // vpush(ret);
1429  return ccdr(first);
1430  } else {
1431  ROS_FATAL("unknown rosparam type!");
1432  return NIL;
1433  }
1434  return NIL;
1435 }
1436 
1438 {
1439  numunion nu;
1440  pointer ret, first;
1441  ret = cons(ctx, NIL, NIL);
1442  first = ret;
1443  vpush(ret);
1444  if ( param_list.getType() == XmlRpc::XmlRpcValue::TypeArray ){
1445  for ( int i = 0; i < param_list.size(); i++){
1446  if ( param_list[i].getType() == XmlRpc::XmlRpcValue::TypeBoolean ){
1447  if ( param_list[i] ){
1448  ccdr(ret) = cons(ctx, T, NIL);
1449  ret = ccdr(ret);
1450  } else {
1451  ccdr(ret) = cons(ctx, NIL, NIL);
1452  ret = ccdr(ret);
1453  }
1454  }
1455  else if ( param_list[i].getType() == XmlRpc::XmlRpcValue::TypeDouble ){
1456  ccdr(ret) = cons(ctx, makeflt((double)param_list[i]), NIL);
1457  ret = ccdr(ret);
1458  }
1459  else if ( param_list[i].getType() == XmlRpc::XmlRpcValue::TypeInt ){
1460  ccdr(ret) = cons(ctx, makeint((int)param_list[i]), NIL);
1461  ret = ccdr(ret);
1462  }
1463  else if ( param_list[i].getType() == XmlRpc::XmlRpcValue::TypeString ){
1464  std::string str = param_list[i];
1465  ccdr(ret) = cons(ctx, makestring((char*)str.c_str(), ((std::string)param_list[i]).length()), NIL);
1466  ret = ccdr(ret);
1467  }
1468  else if ( param_list[i].getType() == XmlRpc::XmlRpcValue::TypeStruct ){
1469  ccdr(ret) = cons(ctx, XmlRpcToEusValue(ctx, param_list[i]), NIL);
1470  ret = ccdr(ret);
1471  }
1472  else {
1473  ROS_FATAL("unknown rosparam type!");
1474  vpop(); // remove vpush(ret)
1475  return NIL;
1476  }
1477  }
1478  vpop(); // remove vpush(ret)
1479  return ccdr(first);
1480  } else if ( param_list.getType() == XmlRpc::XmlRpcValue::TypeStruct ) {
1481  return XmlRpcToEusValue(ctx, param_list);
1482  } else
1483  return (NIL);
1484 }
1485 
1487 {
1488  numunion nu;
1489  string key;
1490 
1491  ckarg2(1,2);
1492  if (isstring(argv[0])) key.assign((char *)get_string(argv[0]));
1493  else error(E_NOSTRING);
1494 
1495  string s;
1496  double d;
1497  bool b;
1498  int i;
1499  pointer ret;
1500  XmlRpc::XmlRpcValue param_list;
1501 
1502  if ( ros::param::get(key, s) ) {
1503  ret = makestring((char *)s.c_str(), s.length());
1504  } else if ( ros::param::get(key, d) ) {
1505  ret = makeflt(d);
1506  } else if ( ros::param::get(key, i) ) {
1507  ret = makeint(i);
1508  } else if ( ros::param::get(key, b) ) {
1509  if ( b == true )
1510  ret = T;
1511  else
1512  ret = NIL;
1513  } else if (ros::param::get(key, param_list)){
1514  ret = XmlRpcToEusList(ctx, param_list);
1515  }else {
1516  if ( n == 2 ) {
1517  ret = COPYOBJ(ctx,1,argv+1);
1518  } else {
1519  ROS_ERROR("unknown ros::param::get, key=%s", key.c_str());
1520  ret = NIL;
1521  }
1522  }
1523  return (ret);
1524 }
1525 
1527 {
1528  numunion nu;
1529  string key;
1530 
1531  ckarg2(1,2);
1532  if (isstring(argv[0])) key.assign((char *)get_string(argv[0]));
1533  else error(E_NOSTRING);
1534 
1535  string s;
1536  double d;
1537  int i;
1538  bool b;
1539  pointer ret;
1540  XmlRpc::XmlRpcValue param_list;
1541  if ( ros::param::getCached(key, s) ) {
1542  ret = makestring((char *)s.c_str(), s.length());
1543  } else if ( ros::param::getCached(key, d) ) {
1544  ret = makeflt(d);
1545  } else if ( ros::param::getCached(key, i) ) {
1546  ret = makeint(i);
1547  } else if ( ros::param::getCached(key, b) ) {
1548  if ( b == true )
1549  ret = T;
1550  else
1551  ret = NIL;
1552  } else if (ros::param::getCached(key, param_list)){
1553  ret = XmlRpcToEusList(ctx, param_list);
1554  } else {
1555  if ( n == 2 ) {
1556  ret = COPYOBJ(ctx,1,argv+1);
1557  } else {
1558  ROS_ERROR("unknown ros::param::get, key=%s", key.c_str());
1559  ret = NIL;
1560  }
1561  }
1562  return (ret);
1563 }
1564 
1566 {
1567  string key;
1568 
1569  ckarg(1);
1570  if (isstring(argv[0])) key.assign((char *)get_string(argv[0]));
1571  else error(E_NOSTRING);
1572 
1573  return((ros::param::has(key))?(T):(NIL));
1574 }
1575 
1577 {
1578  string key;
1579 
1580  ckarg(1);
1581  if (isstring(argv[0])) key.assign((char *)get_string(argv[0]));
1582  else error(E_NOSTRING);
1583 
1584  return((ros::param::del(key))?(T):(NIL));
1585 }
1586 
1588 {
1589  string key, result;
1590 
1591  ckarg(1);
1592  if (isstring(argv[0])) key.assign((char *)get_string(argv[0]));
1593  else error(E_NOSTRING);
1594 
1595  if ( ros::param::search(key, result) ) {
1596  return makestring((char *)result.c_str(), result.length());
1597  }
1598  return(NIL);
1599 }
1600 
1602 {
1603  ckarg(0);
1604 
1605 #if ROS_VERSION_MINIMUM(1,11,17)
1606  std::vector<std::string> keys;
1607  if ( ros::param::getParamNames(keys) ) {
1608  pointer ret = cons(ctx, NIL,NIL), first;
1609  first = ret;
1610  vpush(ret);
1611  for(std::vector<std::string>::iterator it = keys.begin(); it != keys.end(); it++) {
1612  const std::string& key = *it;
1613  ccdr(ret) = cons(ctx, makestring((char *)key.c_str(), key.length()), NIL);
1614  ret = ccdr(ret);
1615  }
1616  vpop();
1617  return ccdr(first);
1618  }
1619 #else
1620  ROS_ERROR("%s : ros::rosparam::getParamNames is not implemented for roscpp %d.%d.%d", __PRETTY_FUNCTION__, ROS_VERSION_MAJOR, ROS_VERSION_MINOR, ROS_VERSION_PATCH);
1621 #endif
1622  return(NIL);
1623 }
1624 
1626 {
1627  ckarg(1);
1628 
1629  string pkg;
1630  if (isstring(argv[0])) pkg.assign((char *)get_string(argv[0]));
1631  else error(E_NOSTRING);
1632 
1633  try {
1634 #ifdef ROSPACK_EXPORT
1635  rospack::Package *p = rp.get_pkg(pkg);
1636  if (p!=NULL) return(makestring((char *)p->path.c_str(),p->path.length()));
1637 #else
1638  std::string path;
1639  if (rp.find(pkg,path)==true) return(makestring((char *)path.c_str(),path.length()));
1640 #endif
1641  } catch (runtime_error &e) {
1642  }
1643  return(NIL);
1644 }
1645 
1647 {
1648  // (ros::rospack-depends package-name)
1649  ckarg(1);
1650 
1651  string pkg;
1652  if (isstring(argv[0])) pkg.assign((char *)get_string(argv[0]));
1653  else error(E_NOSTRING);
1654 
1655  try {
1656  // not sure why need this, otherwise failed in ImportError: /usr/lib/python2.7/lib-dynload/_elementtree.x86_64-linux-gnu.so: undefined symbol: PyExc_RuntimeError
1657  std::vector<std::string> flags;
1658  std::vector<rospack::Stackage*> stackages;
1659  if(!rp.depsOnDetail(pkg, true, stackages, true))
1660  return (NIL);
1661  //
1662  std::vector<std::string> deps;
1663  if (rp.deps(pkg,false,deps)) {
1664  register pointer ret, first;
1665  ret=cons(ctx, NIL, NIL);
1666  first = ret;
1667  vpush(ret);
1668  for (std::vector<std::string>::iterator it = deps.begin() ; it != deps.end(); it++) {
1669  const std::string& dep = *it;
1670  ccdr(ret) = cons(ctx, makestring((char *)dep.c_str(), dep.length()), NIL);
1671  ret = ccdr(ret);
1672  }
1673  vpop(); // vpush(ret)
1674  return ccdr(first);
1675  }
1676  } catch (runtime_error &e) {
1677  }
1678  return(NIL);
1679 }
1680 
1682 {
1683  // (ros::rospack-plugins package-name attibute-name)
1684  ckarg(2);
1685  string pkg, attrib;
1686  pointer ret, first;
1687  if (isstring(argv[0])) pkg.assign((char *)get_string(argv[0]));
1688  else error(E_NOSTRING);
1689  if (isstring(argv[1])) attrib.assign((char *)get_string(argv[1]));
1690  else error(E_NOSTRING);
1691  try {
1692  std::vector<std::string> flags;
1693  if (rp.plugins(pkg, attrib, "", flags)) {
1694  ret = cons(ctx, NIL, NIL);
1695  first = ret;
1696  vpush(ret);
1697  for (size_t i = 0; i < flags.size(); i++) {
1698  // flags[i] = laser_proc /opt/ros/hydro/share/laser_proc/nodelets.xml
1699  std::vector<std::string> parsed_string;
1700  boost::algorithm::split(parsed_string, flags[i], boost::is_any_of(" "));
1701  std::string package = parsed_string[0];
1702  std::string value = parsed_string[1];
1703  pointer tmp = cons(ctx, cons(ctx,
1704  makestring((char*)package.c_str(), package.length()),
1705  makestring((char*)value.c_str(), value.length())),
1706  NIL);
1707  ccdr(ret) = tmp;
1708  ret = tmp;
1709  }
1710  vpop(); // ret
1711  return ccdr(first);
1712  }
1713  else {
1714  return(NIL);
1715  }
1716  }
1717  catch (runtime_error &e) {
1718  }
1719  return(NIL);
1720 }
1721 
1723 {
1724  ckarg(1);
1725  if (!isstring(argv[0])) error(E_NOSTRING);
1726  std::string src;
1727  src.assign((char *)(argv[0]->c.str.chars));
1728  std::string dst = ros::names::resolve(src);
1729  return(makestring((char *)dst.c_str(), dst.length()));
1730 }
1731 
1733 {
1734  ckarg(0);
1735  return(makestring((char *)ros::this_node::getName().c_str(),
1737 }
1738 
1740 {
1741  ckarg(0);
1742  // https://github.com/ros/ros_comm/pull/1100
1743  // Clean the namespace to get rid of double or trailing forward slashes
1745  return(makestring((char *)ns.c_str(), ns.length()));
1746 }
1747 
1749 {
1750  ckarg(2);
1751  string logger;
1752  if (isstring(argv[0])) logger.assign((char *)get_string(argv[0]));
1753  else error(E_NOSTRING);
1754  int log_level = intval(argv[1]);
1756  switch(log_level){
1757  case 1:
1759  break;
1760  case 2:
1762  break;
1763  case 3:
1765  break;
1766  case 4:
1768  break;
1769  case 5:
1771  break;
1772  default:
1773  return (NIL);
1774  }
1775 
1776  bool success = ::ros::console::set_logger_level(logger, level);
1777  if (success)
1778  {
1780  return (T);
1781  }
1782  return (NIL);
1783 }
1784 
1786 {
1787  ckarg(0);
1788 
1790  return makestring((char*)host.c_str(), host.length());
1791 }
1792 
1794 {
1795  ckarg(0);
1796 
1797  ros::V_string nodes;
1798  if ( ! ros::master::getNodes(nodes) ) {
1799  return NIL;
1800  }
1801 
1802  register pointer ret, first;
1803  ret=cons(ctx, NIL, NIL);
1804  first = ret;
1805  vpush(ret);
1806  for (ros::V_string::iterator it = nodes.begin() ; it != nodes.end(); it++) {
1807  std::string node = *it;
1808  ccdr(ret) = cons(ctx, makestring((char *)node.c_str(), node.length()), NIL);
1809  ret = ccdr(ret);
1810  }
1811  vpop(); // vpush(ret)
1812 
1813  return ccdr(first);
1814 }
1815 
1817 {
1818  ckarg(0);
1819 
1820  return makeint(ros::master::getPort());
1821 }
1822 
1824 {
1825  ckarg(0);
1826 
1828  return makestring((char*)uri.c_str(), uri.length());
1829 }
1830 
1832 {
1833  ckarg(0);
1834 
1835  ros::master::V_TopicInfo topics;
1836  if ( !ros::master::getTopics(topics) ) {
1837  return NIL;
1838  }
1839 
1840  register pointer ret, first;
1841  ret=cons(ctx, NIL, NIL);
1842  first = ret;
1843  vpush(ret);
1844  for (ros::master::V_TopicInfo::iterator it = topics.begin() ; it != topics.end(); it++) {
1845  const ros::master::TopicInfo& info = *it;
1846  pointer tmp = cons(ctx,makestring((char*)info.name.c_str(), info.name.length()), makestring((char*)info.datatype.c_str(), info.datatype.length()));
1847  vpush(tmp);
1848  ccdr(ret) = cons(ctx, tmp, NIL);
1849  ret = ccdr(ret);
1850  vpop(); // vpush(tmp)
1851  }
1852  vpop(); // vpush(ret)
1853 
1854  return ccdr(first);
1855 }
1856 
1858 {
1859  pointer _scb, _args;
1860 public:
1861  TimerFunction(pointer scb, pointer args) : _scb(scb), _args(args) {
1862  //context *ctx = current_ctx;
1863  //ROS_WARN("func");prinx(ctx,scb,ERROUT);flushstream(ERROUT);terpri(ERROUT);
1864  //ROS_WARN("argc");prinx(ctx,args,ERROUT);flushstream(ERROUT);terpri(ERROUT);
1865  }
1866  void operator()(const ros::TimerEvent& event)
1867  {
1868  context *ctx = current_ctx;
1869  pointer argp=_args;
1870  int argc=0;
1871 
1872  pointer clsptr = NIL;
1873  for(int i=0; i<nextcix;i++) {
1874  if(!memcmp(classtab[i].def->c.cls.name->c.sym.pname->c.str.chars,(char *)"TIMER-EVENT",11)) {
1875  clsptr = classtab[i].def;
1876  }
1877  }
1878  if ( ! ( issymbol(_scb) || piscode(_scb) || ccar(_scb)==LAMCLOSURE ) ) {
1879  ROS_ERROR("%s : can't find callback function", __PRETTY_FUNCTION__);
1880  }
1881 
1882  pointer tevent = makeobject(clsptr);
1883  csend(ctx,tevent,K_ROSEUS_INIT,0);
1894 
1895  while(argp!=NIL){ ckpush(ccar(argp)); argp=ccdr(argp); argc++;}
1896  vpush((pointer)(tevent));argc++;
1897 
1898  ufuncall(ctx,(ctx->callfp?ctx->callfp->form:NIL),_scb,(pointer)(ctx->vsp-argc),NULL,argc);
1899  while(argc-->0)vpop();
1900 
1901  }
1902 };
1903 
1905 {
1907  numunion nu;
1908  bool oneshot = false;
1909  string groupname;
1910  pointer fncallback = NIL, args;
1911  NodeHandle *lnode = s_node.get();
1912  string fncallname;
1913  float period=ckfltval(argv[0]);
1914 
1915  // period callbackfunc args0 ... argsN [:oneshot oneshot] [:groupname groupname]
1916  bool check_key = true;
1917  while (check_key)
1918  {
1919  if (n > 1 && issymbol(argv[n-2])) {
1920  // ;; oneshot ;;
1921  if (argv[n-2] == K_ROSEUS_ONESHOT && issymbol(argv[n-1])) {
1922  if ( argv[n-1] != NIL ) {
1923  oneshot = true;
1924  }
1925  n -= 2;
1926  }
1927  // ;; groupname ;;
1928  else if (argv[n-2] == K_ROSEUS_GROUPNAME && isstring(argv[n-1])) {
1929  groupname.assign((char *)get_string(argv[n-1]));
1930  map<string, boost::shared_ptr<NodeHandle > >::iterator it = s_mapHandle.find(groupname);
1931  if( it != s_mapHandle.end() ) {
1932  ROS_DEBUG("create-timer with groupname=%s", groupname.c_str());
1933  lnode = (it->second).get();
1934  } else {
1935  ROS_ERROR("Groupname %s is missing. Call (ros::create-nodehandle \"%s\") first.",
1936  groupname.c_str(), groupname.c_str());
1937  return (NIL);
1938  }
1939  n -= 2;
1940  }
1941  else {
1942  check_key = false;
1943  }
1944  }
1945  else {
1946  check_key = false;
1947  }
1948  }
1949 
1950  // ;; functions ;;
1951  if (piscode(argv[1])) { // compiled code
1952  fncallback=argv[1];
1953  std::ostringstream stringstream;
1954  stringstream << reinterpret_cast<long>(argv[2]) << " ";
1955  for (int i=3; i<n;i++)
1956  stringstream << string((char*)(argv[i]->c.sym.pname->c.str.chars)) << " ";
1957  fncallname = stringstream.str();
1958  } else if ((ccar(argv[1]))==LAMCLOSURE) { // uncompiled code
1959  if ( ccar(ccdr(argv[1])) != NIL ) { // function
1960  fncallback=ccar(ccdr(argv[1]));
1961  fncallname = string((char*)(fncallback->c.sym.pname->c.str.chars));
1962  } else { // lambda function
1963  fncallback=argv[1];
1964  std::ostringstream stringstream;
1965  stringstream << reinterpret_cast<long>(argv[1]);
1966  fncallname = stringstream.str();
1967  }
1968  } else {
1969  ROS_ERROR("subscription callback function install error");
1970  }
1971 
1972  // ;; arguments ;;
1973  args=NIL;
1974  for (int i=n-1;i>=2;i--) args=cons(ctx,argv[i],args);
1975 
1976  // avoid gc
1977  pointer p=gensym(ctx);
1978  setval(ctx,intern(ctx,(char*)(p->c.sym.pname->c.str.chars),strlen((char*)(p->c.sym.pname->c.str.chars)),lisppkg),cons(ctx,fncallback,args));
1979 
1980  // ;; store mapTimered
1981  ROS_DEBUG("create timer %s at %f (oneshot=%d) (groupname=%s)", fncallname.c_str(), period, oneshot, groupname.c_str());
1982  s_mapTimered[fncallname] = lnode->createTimer(ros::Duration(period), TimerFunction(fncallback, args), oneshot);
1983 
1984  return (T);
1985 }
1986 
1987 /************************************************************
1988  * __roseus
1989  ************************************************************/
1991 #include "defun.h"
1992 pointer ___roseus(register context *ctx, int n, pointer *argv, pointer env)
1993 {
1994 #ifdef ROSPACK_EXPORT
1995 #else
1996  std::vector<std::string> search_path;
1997  rp.getSearchPathFromEnv(search_path);
1998  rp.crawl(search_path, 1);
1999 #endif
2000 
2001  pointer rospkg,p=Spevalof(PACKAGE);
2002  rospkg=findpkg(makestring("ROS",3));
2003  if (rospkg == 0) rospkg=makepkg(ctx,makestring("ROS", 3),NIL,NIL);
2004  Spevalof(PACKAGE)=rospkg;
2005 
2006  QANON=defvar(ctx,"*ANONYMOUS-NAME*",makeint(ros::init_options::AnonymousName),rospkg);
2007  QNOOUT=defvar(ctx,"*NO-ROSOUT*",makeint(ros::init_options::NoRosout),rospkg);
2008  QROSDEBUG=defvar(ctx,"*ROSDEBUG*",makeint(1),rospkg);
2009  QROSINFO=defvar(ctx,"*ROSINFO*",makeint(2),rospkg);
2010  QROSWARN=defvar(ctx,"*ROSWARN*",makeint(3),rospkg);
2011  QROSERROR=defvar(ctx,"*ROSERROR*",makeint(4),rospkg);
2012  QROSFATAL=defvar(ctx,"*ROSFATAL*",makeint(5),rospkg);
2013  defun(ctx,"SPIN",argv[0],(pointer (*)())ROSEUS_SPIN, "Enter simple event loop");
2014 
2015  defun(ctx,"SPIN-ONCE",argv[0],(pointer (*)())ROSEUS_SPINONCE,
2016  "&optional groupname ;; spin only group\n\n"
2017  "Process a single round of callbacks.\n");
2018  defun(ctx,"TIME-NOW-RAW",argv[0],(pointer (*)())ROSEUS_TIME_NOW, "");
2019  defun(ctx,"RATE",argv[0],(pointer (*)())ROSEUS_RATE, "frequency\n\n" "Construct ros timer for periodic sleeps");
2020  defun(ctx,"SLEEP",argv[0],(pointer (*)())ROSEUS_SLEEP, "Sleeps for any leftover time in a cycle. Calculated from the last time sleep, reset, or the constructor was called.");
2021  defun(ctx,"DURATION-SLEEP",argv[0],(pointer (*)())ROSEUS_DURATION_SLEEP, "second\n\nSleeps for amount of the time specified by this duration.");
2022  defun(ctx,"OK",argv[0],(pointer (*)())ROSEUS_OK, "Check whether it's time to exit. ");
2023 
2024  defun(ctx,"ROS-DEBUG",argv[0],(pointer (*)())ROSEUS_ROSDEBUG,
2025  "write mesage to debug output\n"
2026  "\n"
2027  " (ros::ros-debug \"this is error ~A\" 0)\n");
2028  defun(ctx,"ROS-INFO",argv[0],(pointer (*)())ROSEUS_ROSINFO, "write mesage to info output");
2029  defun(ctx,"ROS-WARN",argv[0],(pointer (*)())ROSEUS_ROSWARN, "write mesage to warn output");
2030  defun(ctx,"ROS-ERROR",argv[0],(pointer (*)())ROSEUS_ROSERROR, "write mesage to error output");
2031  defun(ctx,"ROS-FATAL",argv[0],(pointer (*)())ROSEUS_ROSFATAL, "write mesage to fatal output");
2032  defun(ctx,"EXIT",argv[0],(pointer (*)())ROSEUS_EXIT, "Exit ros clinet");
2033 
2034  defun(ctx,"SUBSCRIBE",argv[0],(pointer (*)())ROSEUS_SUBSCRIBE,
2035  "topicname message_type callbackfunc args0 ... argsN &optional (queuesize 1) &key (:groupname groupname)\n\n"
2036  "Subscribe to a topic, version for class member function with bare pointer.\n"
2037  "This method connects to the master to register interest in a given topic. The node will automatically be connected with publishers on this topic. On each message receipt, fp is invoked and passed a shared pointer to the message received. This message should not be changed in place, as it is shared with any other subscriptions to this topic.\n"
2038  "\n"
2039  "This version of subscribe is a convenience function for using function, member function, lmabda function:\n"
2040  " ;; callback function\n"
2041  " (defun string-cb (msg) (print (list 'cb (sys::thread-self) (send msg :data))))\n"
2042  " (ros::subscribe \"chatter\" std_msgs::string #'string-cb)\n"
2043  " ;; lambda function\n"
2044  " (ros::subscribe \"chatter\" std_msgs::string\n"
2045  " #'(lambda (msg) (ros::ros-info\n"
2046  " (format nil \"I heard ~A\" (send msg :data)))))\n"
2047  " ;; method call\n"
2048  " (defclass string-cb-class\n"
2049  " :super propertied-object\n"
2050  " :slots ())\n"
2051  " (defmethod string-cb-class\n"
2052  " (:init () (ros::subscribe \"chatter\" std_msgs::string #'send self :string-cb))\n"
2053  " (:string-cb (msg) (print (list 'cb self (send msg :data)))))\n"
2054  " (setq m (instance string-cb-class :init))\n"
2055  );
2056  defun(ctx,"UNSUBSCRIBE",argv[0],(pointer (*)())ROSEUS_UNSUBSCRIBE, "topicname\n\n""Unsubscribe topic");
2057  defun(ctx,"GET-NUM-PUBLISHERS",argv[0],(pointer (*)())ROSEUS_GETNUMPUBLISHERS, "Returns the number of publishers this subscriber is connected to. ");
2058  defun(ctx,"GET-TOPIC-SUBSCRIBER",argv[0],(pointer (*)())ROSEUS_GETTOPICSUBSCRIBER, "topicname\n\n""Retuns the name of topic if it already subscribed");
2059  defun(ctx,"ADVERTISE",argv[0],(pointer (*)())ROSEUS_ADVERTISE,
2060  "topic message_class &optional (queuesize 1) (latch nil)\n"
2061  "Advertise a topic.\n"
2062  "This call connects to the master to publicize that the node will be publishing messages on the given topic. This method returns a Publisher that allows you to publish a message on this topic.\n"
2063  " (ros::advertise \"chatter\" std_msgs::string 1)");
2064  defun(ctx,"UNADVERTISE",argv[0],(pointer (*)())ROSEUS_UNADVERTISE, "Unadvertise topic");
2065  defun(ctx,"PUBLISH",argv[0],(pointer (*)())ROSEUS_PUBLISH,
2066  "topic message\n\n"
2067  "Publish a message on the topic\n"
2068  " (ros::roseus \"talker\")\n"
2069  " (ros::advertise \"chatter\" std_msgs::string 1)\n"
2070  " (ros::rate 100)\n"
2071  " (while (ros::ok)\n"
2072  " (setq msg (instance std_msgs::string :init))\n"
2073  " (send msg :data (format nil \"hello world ~a\" (send (ros::time-now) :sec-nsec)))\n"
2074  " (ros::publish \"chatter\" msg)\n"
2075  " (ros::sleep))\n");
2076  defun(ctx,"GET-NUM-SUBSCRIBERS",argv[0],(pointer (*)())ROSEUS_GETNUMSUBSCRIBERS, "Retuns number of subscribers this publish is connected to");
2077  defun(ctx,"GET-TOPIC-PUBLISHER",argv[0],(pointer (*)())ROSEUS_GETTOPICPUBLISHER, "topicname\n\n""Retuns the name of topic if it already published");
2078 
2079  defun(ctx,"WAIT-FOR-SERVICE",argv[0],(pointer (*)())ROSEUS_WAIT_FOR_SERVICE, "servicename &optional timeout\n\n""Wait for a service to be advertised and available. Blocks until it is. If the timeout is -1, wait until the node is shutdown. Otherwise it wait for timeout seconds");
2080  defun(ctx,"SERVICE-EXISTS", argv[0], (pointer (*)())ROSEUS_SERVICE_EXISTS, "servicename\n\n""Checks if a service is both advertised and available.");
2081  defun(ctx,"SERVICE-CALL",argv[0],(pointer (*)())ROSEUS_SERVICE_CALL,
2082  "servicename message_type &optional persist\n\n"
2083  "Invoke RPC service\n"
2084  " (ros::roseus \"add_two_ints_client\")\n"
2085  " (ros::wait-for-service \"add_two_ints\")\n"
2086  " (setq req (instance roseus::AddTwoIntsRequest :init))\n"
2087  " (send req :a (random 10))\n"
2088  " (send req :b (random 20))\n"
2089  " (setq res (ros::service-call \"add_two_ints\" req t))\n"
2090  " (format t \"~d + ~d = ~d~~%\" (send req :a) (send req :b) (send res :sum))\n");
2091  defun(ctx,"ADVERTISE-SERVICE",argv[0],(pointer (*)())ROSEUS_ADVERTISE_SERVICE,
2092  "servicename message_type callbackfunc args0 ... argsN &key (:groupname groupname)\n\n"
2093  "Advertise a service\n"
2094  " (ros::advertise-service \"add_two_ints\" roseus::AddTwoInts #'add-two-ints)");
2095  defun(ctx,"UNADVERTISE-SERVICE",argv[0],(pointer (*)())ROSEUS_UNADVERTISE_SERVICE, "Unadvertise service");
2096 
2097  defun(ctx,"SET-PARAM",argv[0],(pointer (*)())ROSEUS_SET_PARAM, "key value\n\n""Set parameter");
2098  defun(ctx,"GET-PARAM",argv[0],(pointer (*)())ROSEUS_GET_PARAM, "key\n\n""Get parameter");
2099  defun(ctx,"GET-PARAM-CACHED",argv[0],(pointer (*)())ROSEUS_GET_PARAM_CACHED, "Get chached parameter");
2100  defun(ctx,"HAS-PARAM",argv[0],(pointer (*)())ROSEUS_HAS_PARAM, "Check whether a parameter exists on the parameter server.");
2101  defun(ctx,"DELETE-PARAM",argv[0],(pointer (*)())ROSEUS_DELETE_PARAM, "key\n\n""Delete parameter from server");
2102  defun(ctx,"SEARCH-PARAM",argv[0],(pointer (*)())ROSEUS_SEARCH_PARAM, "key\n\n""Search up the tree for a parameter with a given key. This version defaults to starting in the current node's name.");
2103  defun(ctx,"LIST-PARAM",argv[0],(pointer (*)())ROSEUS_LIST_PARAM, "Get the list of all the parameters in the server");
2104 
2105  defun(ctx,"ROSPACK-FIND",argv[0],(pointer (*)())ROSEUS_ROSPACK_FIND, "Returns ros package path");
2106  defun(ctx,"ROSPACK-PLUGINS",argv[0],(pointer (*)())ROSEUS_ROSPACK_PLUGINS, "Returns plugins of ros packages");
2107  defun(ctx,"ROSPACK-DEPENDS",argv[0],(pointer (*)())ROSEUS_ROSPACK_DEPENDS, "Returns ros package dependencies list");
2108  defun(ctx,"RESOLVE-NAME",argv[0],(pointer (*)())ROSEUS_RESOLVE_NAME, "Returns ros resolved name");
2109  defun(ctx,"GET-NAME",argv[0],(pointer (*)())ROSEUS_GETNAME, "Returns current node name");
2110  defun(ctx,"GET-NAMESPACE",argv[0],(pointer (*)())ROSEUS_GETNAMESPACE, "Returns current node name space");
2111 
2112  defun(ctx,"ROSEUS-RAW",argv[0],(pointer (*)())ROSEUS, "");
2113  defun(ctx,"CREATE-NODEHANDLE", argv[0], (pointer (*)())ROSEUS_CREATE_NODEHANDLE, "groupname &optional namespace ;;\n\n"
2114  "Create ros NodeHandle with given group name. \n"
2115  "\n"
2116  " (ros::roseus \"test\")\n"
2117  " (ros::create-nodehandle \"mygroup\")\n"
2118  " (ros::subscribe \"/test\" std_msgs::String #'(lambda (m) (print m)) :groupname \"mygroup\")\n"
2119  " (ros::create-timer 0.1 #'(lambda (event) (print \"timer called\")) :groupname \"mygroup\")\n"
2120  " (while (ros::ok) (ros::spin-once \"mygroup\"))\n");
2121  defun(ctx,"SET-LOGGER-LEVEL",argv[0],(pointer (*)())ROSEUS_SET_LOGGER_LEVEL, "");
2122 
2123  defun(ctx,"GET-HOST",argv[0],(pointer (*)())ROSEUS_GET_HOST, "Get the hostname where the master runs.");
2124  defun(ctx,"GET-NODES",argv[0],(pointer (*)())ROSEUS_GET_NODES, "Retreives the currently-known list of nodes from the master.");
2125  defun(ctx,"GET-PORT",argv[0],(pointer (*)())ROSEUS_GET_PORT, "Get the port where the master runs.");
2126  defun(ctx,"GET-URI",argv[0],(pointer (*)())ROSEUS_GET_URI, "Get the full URI to the master ");
2127  defun(ctx,"GET-TOPICS",argv[0],(pointer (*)())ROSEUS_GET_TOPICS, "Get the list of topics that are being published by all nodes.");
2128 
2129  defun(ctx,"CREATE-TIMER",argv[0],(pointer (*)())ROSEUS_CREATE_TIMER,
2130  "period callbackfunc args0 ... argsN &key (:oneshot oneshot) (:groupname groupname)\n\n"
2131  "Create periodic callbacks.\n"
2132  "\n"
2133  " ;; callback function\n"
2134  " (defun timer-cb (event) (print \"timer callback called.\")\n"
2135  " (defun oneshot-timer-cb (event) (print \"timer callback called. oneshot.\")\n"
2136  " (ros::create-timer 0.1 #'timer-cb)\n"
2137  " (ros::create-timer 0.1 #'oneshot-timer-cb :oneshot t)\n"
2138  " ;; lambda function\n"
2139  " (ros::create-timer 0.1 #'(lambda (event) (print \"timer callback called\")))\n"
2140  " ;; method call\n"
2141  " (defclass timer-cb-class\n"
2142  " :super propertied-object\n"
2143  " :slots ())\n"
2144  " (defmethod timer-cb-class\n"
2145  " (:init () (ros::create-timer 0.1 #'send self :timer-cb))\n"
2146  " (:timer-cb (event) (print \"timer callback called\")))\n"
2147  " (setq m (instance timer-cb-class :init))\n"
2148  );
2149 
2150  pointer_update(Spevalof(PACKAGE),p);
2151 
2152  pointer l;
2153  l=makestring(REPOVERSION,strlen(REPOVERSION));
2154  vpush(l);
2155  l=stacknlist(ctx,1);
2156  QREPOVERSION=defvar(ctx, "ROSEUS-REPO-VERSION", l, rospkg);
2157 
2158  M_string remappings;
2159  pointer argp = speval(intern(ctx,"*EUSTOP-ARGUMENT*", strlen("*EUSTOP-ARGUMENT*"),lisppkg));
2160  while(argp!=NIL) {
2161  std::string arg = string((char *)(ccar(argp)->c.str.chars));
2162  // copy from roscpp/src/init.cpp : 432
2163  size_t pos = arg.find(":=");
2164  if (pos != std::string::npos) {
2165  std::string local_name = arg.substr(0, pos);
2166  std::string external_name = arg.substr(pos + 2);
2167  remappings[local_name] = external_name;
2168  }
2169  argp=ccdr(argp);
2170  }
2171  ros::master::init(remappings);
2172  //ros::param::init(remappings);
2173 
2174  return 0;
2175 }
2176 
ros::init_options::AnonymousName
AnonymousName
RoseusStaticData::mapHandle
map< string, boost::shared_ptr< NodeHandle > > mapHandle
for grouping nodehandle
Definition: roseus.cpp:139
XmlRpc::XmlRpcValue::size
int size() const
response
const std::string response
ros::ServiceCallbackHelperCallParams::response
SerializedMessage response
result
def result
XmlRpcToEusValue
pointer XmlRpcToEusValue(register context *ctx, XmlRpc::XmlRpcValue rpc_value)
Definition: roseus.cpp:1384
ros::init_options::NoRosout
NoRosout
EuslispSubscriptionCallbackHelper
Definition: roseus.cpp:351
numunion
RoseusStaticData
Definition: roseus.cpp:126
ros::WallDuration::sleep
bool sleep() const
RoseusStaticData::mapServiced
map< string, boost::shared_ptr< ServiceServer > > mapServiced
subscribed topics
Definition: roseus.cpp:136
node_handle.h
ros::SerializedMessage
TimerFunction::_scb
pointer _scb
Definition: roseus.cpp:1859
ROSEUS_ROSPACK_DEPENDS
pointer ROSEUS_ROSPACK_DEPENDS(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:1646
ros::serialization::Serializer< EuslispMessage >::read
static void read(Stream &stream, boost::call_traits< EuslispMessage >::reference t)
Definition: roseus.cpp:338
K_ROSEUS_REQUEST
pointer K_ROSEUS_REQUEST
Definition: roseus.cpp:152
ros::SerializedMessage::message_start
uint8_t * message_start
ros::this_node::getNamespace
const ROSCPP_DECL std::string & getNamespace()
NIL
pointer NIL
s_mapHandle
#define s_mapHandle
Definition: roseus.cpp:150
mkstream
pointer mkstream(context *, pointer, pointer)
ros::ServiceCallbackHelperCallParams::request
SerializedMessage request
ros::service::exists
ROSCPP_DECL bool exists(const std::string &service_name, bool print_failure_reason)
call
bool call(const std::string &service_name, MReq &req, MRes &res)
RoseusStaticData::RoseusStaticData
RoseusStaticData()
Definition: roseus.cpp:129
l
long l
ros::console::levels::Error
Error
K_ROSEUS_SEC
pointer K_ROSEUS_SEC
Definition: roseus.cpp:152
deserialize
void deserialize(Stream &stream, boost::array< T, N > &t)
ros::serialization::Serializer< EuslispMessage >::write
static void write(Stream &stream, boost::call_traits< EuslispMessage >::param_type t)
Definition: roseus.cpp:333
ROSEUS_GET_PARAM_CACHED
pointer ROSEUS_GET_PARAM_CACHED(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:1526
ROSEUS_GET_PARAM
pointer ROSEUS_GET_PARAM(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:1486
ROSEUS_SLEEP
pointer ROSEUS_SLEEP(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:771
ROSEUS_HAS_PARAM
pointer ROSEUS_HAS_PARAM(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:1565
s_mapServiced
#define s_mapServiced
Definition: roseus.cpp:148
boost::shared_ptr< ros::NodeHandle >
makeint
pointer makeint(eusinteger_t v)
rospack::Rosstackage::getSearchPathFromEnv
bool getSearchPathFromEnv(std::vector< std::string > &sp)
eus.h
ros::master::g_uri
std::string g_uri
ros::serialization::Serializer< EuslispMessage >::serializedLength
static uint32_t serializedLength(boost::call_traits< EuslispMessage >::param_type t)
Definition: roseus.cpp:341
EuslispMessage::_message
pointer _message
Definition: roseus.cpp:213
rospack::Rospack
ros::init
ROSCPP_DECL void init(const M_string &remappings, const std::string &name, uint32_t options=0)
isInstalledCheck
#define isInstalledCheck
Definition: roseus.cpp:123
ros::TimerEvent::last_real
Time last_real
ROSEUS_SERVICE_CALL
pointer ROSEUS_SERVICE_CALL(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:1115
get
ROSCPP_DECL bool get(const std::string &key, bool &b)
context
ROSEUS_GETTOPICPUBLISHER
pointer ROSEUS_GETTOPICPUBLISHER(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:1058
ros::master::init
void init(const M_string &remappings)
QROSFATAL
pointer QROSFATAL
Definition: roseus.cpp:152
LAMCLOSURE
pointer LAMCLOSURE
defkeyword
pointer defkeyword(context *, char *)
s
short s
ROSEUS_CREATE_NODEHANDLE
pointer ROSEUS_CREATE_NODEHANDLE(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:673
RoseusStaticData::mapSubscribed
map< string, boost::shared_ptr< Subscriber > > mapSubscribed
subscribed topics
Definition: roseus.cpp:135
ros::master::getPort
ROSCPP_DECL uint32_t getPort()
QROSWARN
pointer QROSWARN
Definition: roseus.cpp:152
XmlRpc::XmlRpcValue::fromXml
bool fromXml(std::string const &valueXml, int *offset)
ros
K_ROSEUS_LAST_DURATION
pointer K_ROSEUS_LAST_DURATION
Definition: roseus.cpp:152
K_ROSEUS_GROUPNAME
pointer K_ROSEUS_GROUPNAME
Definition: roseus.cpp:152
PACKAGE
pointer PACKAGE
K_ROSEUS_MD5SUM
pointer K_ROSEUS_MD5SUM
Definition: roseus.cpp:152
time.h
makevector
pointer makevector(pointer, int)
ros::param::init
void init(const M_string &remappings)
K_ROSEUS_CONNECTION_HEADER
pointer K_ROSEUS_CONNECTION_HEADER
Definition: roseus.cpp:152
serializationLength
uint32_t serializationLength(const boost::array< T, N > &t)
ROSEUS_RESOLVE_NAME
pointer ROSEUS_RESOLVE_NAME(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:1722
EuslispMessage::serialize
virtual uint8_t * serialize(uint8_t *writePtr, uint32_t seqid) const
Definition: roseus.cpp:265
ros::spinOnce
ROSCPP_DECL void spinOnce()
DurationBase< WallDuration >::nsec
int32_t nsec
XmlRpc::XmlRpcValue::TypeInt
TypeInt
ros::serialization::Stream
service.h
ros::names::clean
ROSCPP_DECL std::string clean(const std::string &name)
add_module_initializer
void add_module_initializer(char *, pointer(*)())
init.h
ckfltval
float ckfltval()
ros::shutdown
ROSCPP_DECL void shutdown()
T
pointer T
ros::NodeHandle::advertiseService
ServiceServer advertiseService(AdvertiseServiceOptions &ops)
StoreConnectionHeader
void StoreConnectionHeader(EuslispMessage *eus_msg)
Definition: roseus.cpp:307
rospack::Rosstackage::plugins
bool plugins(const std::string &name, const std::string &attrib, const std::string &top, std::vector< std::string > &flags)
ros::TimerEvent::last_duration
WallDuration last_duration
s_mapSubscribed
#define s_mapSubscribed
Definition: roseus.cpp:147
rospack::Rosstackage::find
bool find(const std::string &name, std::string &path)
ROSEUS_GETNAME
pointer ROSEUS_GETNAME(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:1732
EuslispMessage::__getDataType
virtual const string __getDataType() const
Definition: roseus.cpp:240
ros::ServiceManager::instance
static const ServiceManagerPtr & instance()
s_rate
#define s_rate
Definition: roseus.cpp:145
ROSEUS_GET_PORT
pointer ROSEUS_GET_PORT(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:1816
E_MISMATCHARG
E_MISMATCHARG
ROSEUS_GET_NODES
pointer ROSEUS_GET_NODES(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:1793
ros::AdvertiseServiceOptions::md5sum
std::string md5sum
ROSEUS_GETTOPICSUBSCRIBER
pointer ROSEUS_GETTOPICSUBSCRIBER(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:930
string::length
pointer length
cell::cellunion::sym
struct symbol sym
makestring
pointer makestring(char *, int)
ros::AdvertiseServiceOptions::helper
ServiceCallbackHelperPtr helper
ROSEUS_OK
pointer ROSEUS_OK(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:788
roseusSignalHandler
void roseusSignalHandler(int sig)
Definition: roseus.cpp:562
s_node
#define s_node
Definition: roseus.cpp:144
ROSEUS_SERVICE_EXISTS
pointer ROSEUS_SERVICE_EXISTS(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:1101
symbol::pname
pointer pname
ros::master::getTopics
ROSCPP_DECL bool getTopics(V_TopicInfo &topics)
ROSEUS_UNADVERTISE
pointer ROSEUS_UNADVERTISE(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:989
makeobject
pointer makeobject(pointer)
ROSEUS_GET_HOST
pointer ROSEUS_GET_HOST(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:1785
EuslispMessage::deserialize
virtual uint8_t * deserialize(uint8_t *readPtr, uint32_t sz)
Definition: roseus.cpp:283
ROSEUS_SUBSCRIBE
pointer ROSEUS_SUBSCRIBE(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:837
E_NOSTRING
E_NOSTRING
ros::AdvertiseOptions
ros::ok
ROSCPP_DECL bool ok()
ros::param::del
ROSCPP_DECL bool del(const std::string &key)
stream
cell::cellunion::ivec
struct intvector ivec
ROSEUS_GETNUMPUBLISHERS
pointer ROSEUS_GETNUMPUBLISHERS(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:904
ros::names::resolve
ROSCPP_DECL std::string resolve(const std::string &name, bool remap=true)
XmlRpc::XmlRpcValue::TypeStruct
TypeStruct
makebuffer
pointer makebuffer(int)
K_ROSEUS_DEFINITION
pointer K_ROSEUS_DEFINITION
Definition: roseus.cpp:152
ros::AdvertiseServiceOptions::datatype
std::string datatype
TimerFunction::operator()
void operator()(const ros::TimerEvent &event)
Definition: roseus.cpp:1866
service_manager.h
QROSDEBUG
pointer QROSDEBUG
Definition: roseus.cpp:152
rospack::ROSPack
get_string
byte * get_string(register pointer s)
Definition: roseus.cpp:98
_exit
else _exit(ckintval(argv[0]))
ROSEUS_SET_PARAM
pointer ROSEUS_SET_PARAM(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:1369
intern
pointer intern(context *, char *, int, pointer)
EuslispMessage::__getServiceDataType
virtual const string __getServiceDataType() const
Definition: roseus.cpp:249
string::chars
byte chars[1]
TimerFunction
Definition: roseus.cpp:1857
cons
EuslispMessage::_connection_header
boost::shared_ptr< map< string, string > > _connection_header
Definition: roseus.cpp:214
QREPOVERSION
pointer QREPOVERSION
Definition: roseus.cpp:152
cell::c
union cell::cellunion c
rospack::Rosstackage::crawl
void crawl(std::vector< std::string > search_path, bool force)
gensym
pointer gensym(context *)
K_ROSEUS_SERIALIZATION_LENGTH
pointer K_ROSEUS_SERIALIZATION_LENGTH
Definition: roseus.cpp:152
defvar
pointer defvar(context *, char *, pointer, pointer)
K_ROSEUS_NSEC
pointer K_ROSEUS_NSEC
Definition: roseus.cpp:152
ros::param::getCached
ROSCPP_DECL bool getCached(const std::string &key, bool &b)
class_desc::def
pointer def
ros::console::levels::Debug
Debug
EuslispMessage
Definition: roseus.cpp:210
s_bInstalled
static bool s_bInstalled
Definition: roseus.cpp:143
XmlRpc::XmlRpcValue::TypeDouble
TypeDouble
flushstream
int flushstream(pointer)
ROSEUS_WAIT_FOR_SERVICE
pointer ROSEUS_WAIT_FOR_SERVICE(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:1081
XmlRpcToEusList
pointer XmlRpcToEusList(register context *ctx, XmlRpc::XmlRpcValue param_list)
Definition: roseus.cpp:1437
ROSEUS_GET_TOPICS
pointer ROSEUS_GET_TOPICS(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:1831
intval
eusinteger_t intval(pointer p)
ros::AdvertiseOptions::latch
bool latch
ROS_DEBUG
#define ROS_DEBUG(...)
NULL
#define NULL
ros::master::V_TopicInfo
std::vector< TopicInfo > V_TopicInfo
K_ROSEUS_SERIALIZE
pointer K_ROSEUS_SERIALIZE
Definition: roseus.cpp:152
fltval
float fltval()
COPYOBJ
pointer COPYOBJ(context *, int, pointer *)
K_ROSEUS_DATATYPE
pointer K_ROSEUS_DATATYPE
Definition: roseus.cpp:152
rp
rospack::Rospack rp
Definition: roseus.cpp:90
XmlRpc::XmlRpcValue::TypeString
TypeString
stream::count
pointer count
euscontexts
context * euscontexts[MAXTHREAD]
classtab
struct class_desc classtab[MAXCLASS]
ros::AdvertiseServiceOptions::req_datatype
std::string req_datatype
argv
ROS_INFO ROS_ERROR int pointer * argv
Definition: roseus.cpp:819
server
server
ROSEUS_ROSPACK_FIND
pointer ROSEUS_ROSPACK_FIND(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:1625
ROSEUS_GETNUMSUBSCRIBERS
pointer ROSEUS_GETNUMSUBSCRIBERS(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:1032
findpkg
pointer findpkg()
d
d
ros::param::search
ROSCPP_DECL bool search(const std::string &key, std::string &result)
ROS_WARN
#define ROS_WARN(...)
E_NOSEQ
E_NOSEQ
rospack::Rosstackage::depsOnDetail
bool depsOnDetail(const std::string &name, bool direct, std::vector< Stackage * > &deps, bool ignore_missing=false)
prinx
pointer prinx(context *, pointer, pointer)
ros::master::getHost
const ROSCPP_DECL std::string & getHost()
ROSEUS_UNSUBSCRIBE
pointer ROSEUS_UNSUBSCRIBE(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:891
register_roseus
void register_roseus()
Definition: roseus.cpp:93
ROSEUS_DURATION_SLEEP
pointer ROSEUS_DURATION_SLEEP(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:778
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
rate.h
ros::AdvertiseServiceOptions
ros::NodeHandle
ros::TimerEvent
ROSEUS_TIME_NOW
pointer ROSEUS_TIME_NOW(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:747
ROSEUS_DELETE_PARAM
pointer ROSEUS_DELETE_PARAM(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:1576
ROSEUS_CREATE_TIMER
pointer ROSEUS_CREATE_TIMER(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:1904
XmlRpc::XmlRpcValue::iterator
ValueStruct::iterator iterator
ROSEUS_RATE
pointer ROSEUS_RATE(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:761
QROSERROR
pointer QROSERROR
Definition: roseus.cpp:152
ros::SerializedMessage::num_bytes
size_t num_bytes
K_ROSEUS_LAST_EXPECTED
pointer K_ROSEUS_LAST_EXPECTED
Definition: roseus.cpp:152
nextcix
int nextcix
ros::TimerEvent::current_real
Time current_real
ROSEUS_ROSPACK_PLUGINS
pointer ROSEUS_ROSPACK_PLUGINS(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:1681
TimeBase< Time, Duration >::sec
uint32_t sec
EuslispServiceCallbackHelper
Definition: roseus.cpp:431
ros::Publisher
XmlRpc::XmlRpcValue::end
iterator end()
cell::cellunion::stream
struct stream stream
ros::SubscriptionCallbackHelperDeserializeParams
ros::console::levels::Level
Level
ros::console::levels::Info
Info
XmlRpc::XmlRpcValue::getType
const Type & getType() const
ROSEUS_LIST_PARAM
pointer ROSEUS_LIST_PARAM(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:1601
TimeBase< Time, Duration >::nsec
uint32_t nsec
ROS_FATAL
#define ROS_FATAL(...)
csend
pointer csend(va_alist)
ros::TimerEvent::profile
struct ros::TimerEvent::@0 profile
lisppkg
pointer lisppkg
callback_queue.h
rospack::Rosstackage::deps
bool deps(const std::string &name, bool direct, std::vector< std::string > &deps)
ros::Connection::Destructing
Destructing
XmlRpc::XmlRpcValue::TypeArray
TypeArray
ROSEUS_SPIN
pointer ROSEUS_SPIN(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:709
makeflt
pointer makeflt()
error
pointer error(enum errorcode ec,...) pointer error(va_alist) va_dcl
ROSEUS_ADVERTISE
pointer ROSEUS_ADVERTISE(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:950
ros::param::set
ROSCPP_DECL void set(const std::string &key, bool b)
ros::AdvertiseServiceOptions::service
std::string service
EuslispMessage::~EuslispMessage
virtual ~EuslispMessage()
Definition: roseus.cpp:234
ros::this_node::getName
const ROSCPP_DECL std::string & getName()
K_ROSEUS_CURRENT_EXPECTED
pointer K_ROSEUS_CURRENT_EXPECTED
Definition: roseus.cpp:152
ERROUT
pointer ERROUT
ros::master::getNodes
ROSCPP_DECL bool getNodes(V_string &nodes)
EuslispMessage::__getMD5Sum
virtual const string __getMD5Sum() const
Definition: roseus.cpp:243
intvector::iv
long iv[1]
RoseusStaticData::mapAdvertised
map< string, boost::shared_ptr< Publisher > > mapAdvertised
advertised topics
Definition: roseus.cpp:134
ros::ServiceCallbackHelper
this_node.h
connection.h
K_OUT
pointer K_OUT
QNOOUT
pointer QNOOUT
Definition: roseus.cpp:152
ROSEUS_SEARCH_PARAM
pointer ROSEUS_SEARCH_PARAM(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:1587
SETSLOT
pointer SETSLOT(context *, int, pointer *)
ros::SubscribeOptions
ros::console::levels::Fatal
Fatal
ros::console::notifyLoggerLevelsChanged
ROSCONSOLE_DECL void notifyLoggerLevelsChanged()
ros::Time
findmethod
pointer findmethod(context *, pointer, pointer, pointer *)
cell
ros::CallbackQueue
getString
string getString(pointer message, pointer method)
Definition: roseus.cpp:159
makepkg
pointer makepkg(context *, pointer, pointer, pointer)
context::vsp
pointer * vsp
EusValueToXmlRpc
void EusValueToXmlRpc(register context *ctx, pointer argp, XmlRpc::XmlRpcValue &rpc_value)
Definition: roseus.cpp:1279
std
length
TFSIMD_FORCE_INLINE tfScalar length(const Quaternion &q)
string
#define string
Definition: roseus.cpp:83
ros::master::TopicInfo::datatype
std::string datatype
stacknlist
pointer stacknlist(context *, int)
ROSEUS_UNADVERTISE_SERVICE
pointer ROSEUS_UNADVERTISE_SERVICE(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:1265
EuslispMessage::serializationLength
uint32_t serializationLength() const
Definition: roseus.cpp:256
ROS_ERROR
#define ROS_ERROR(...)
rospack.h
XmlRpc::XmlRpcValue::setSize
void setSize(int size)
ros::serialization::serializeMessage
SerializedMessage serializeMessage(const M &message)
datatype
const char * datatype()
ros::isShuttingDown
ROSCPP_DECL bool isShuttingDown()
ros::service::waitForService
ROSCPP_DECL bool waitForService(const std::string &service_name, int32_t timeout)
ros::InvalidNameException
stream::buffer
pointer buffer
ros::TimerEvent::last_expected
Time last_expected
defun
pointer defun(context *, char *, pointer, pointer(*)(), char *)
TimerFunction::TimerFunction
TimerFunction(pointer scb, pointer args)
Definition: roseus.cpp:1861
ROSEUS_ADVERTISE_SERVICE
pointer ROSEUS_ADVERTISE_SERVICE(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:1201
context::callfp
struct callframe * callfp
RoseusStaticData::node
boost::shared_ptr< ros::NodeHandle > node
Definition: roseus.cpp:132
C_INTVECTOR
pointer C_INTVECTOR
ros::param::getParamNames
ROSCPP_DECL bool getParamNames(std::vector< std::string > &keys)
param
T param(const std::string &param_name, const T &default_val)
RoseusStaticData::mapTimered
map< string, Timer > mapTimered
subscribed timers
Definition: roseus.cpp:137
ros::console::levels::Warn
Warn
___roseus
pointer ___roseus(register context *ctx, int n, pointer *argv, pointer env)
Definition: roseus.cpp:1992
ros::TimerEvent::current_expected
Time current_expected
s_mapAdvertised
#define s_mapAdvertised
Definition: roseus.cpp:146
ros::Rate
EuslispMessage::EuslispMessage
EuslispMessage(pointer message)
Definition: roseus.cpp:216
EuslispMessage::replaceContents
virtual void replaceContents(pointer newMessage)
Definition: roseus.cpp:236
s_staticdata
static RoseusStaticData s_staticdata
Definition: roseus.cpp:142
n
ROS_INFO ROS_ERROR int n
Definition: roseus.cpp:818
ros::ServiceServer
ros::V_string
std::vector< std::string > V_string
terpri
void terpri(pointer)
ros::Duration::sleep
bool sleep() const
ros::ServiceCallbackHelperCallParams
ros::param::has
ROSCPP_DECL bool has(const std::string &key)
ufuncall
pointer ufuncall(context *, pointer, pointer, pointer, struct bindframe *, int)
XmlRpc::XmlRpcValue::begin
iterator begin()
K_ROSEUS_INIT
pointer K_ROSEUS_INIT
Definition: roseus.cpp:152
args
XmlRpc::XmlRpcValue::TypeBoolean
TypeBoolean
RoseusStaticData::rate
boost::shared_ptr< ros::Rate > rate
Definition: roseus.cpp:133
package
ros::WallDuration
ROSEUS_GET_URI
pointer ROSEUS_GET_URI(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:1823
header
const std::string header
hasHeader
bool hasHeader()
ros::SubscribeOptions::helper
SubscriptionCallbackHelperPtr helper
ros::Subscriber
setval
pointer setval(context *, pointer, pointer)
ROS_INFO
#define ROS_INFO(...)
ROSEUS_GETNAMESPACE
pointer ROSEUS_GETNAMESPACE(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:1739
ros::master::TopicInfo
ros::SubscriptionCallbackHelper
ROS_ASSERT
#define ROS_ASSERT(cond)
a
char a[26]
ROSEUS_PUBLISH
pointer ROSEUS_PUBLISH(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:1002
ros::AdvertiseServiceOptions::res_datatype
std::string res_datatype
ros::NodeHandle::createTimer
Timer createTimer(Duration period, const TimerCallback &callback, bool oneshot=false, bool autostart=true) const
ros::master::getURI
const ROSCPP_DECL std::string & getURI()
K_ROSEUS_RESPONSE
pointer K_ROSEUS_RESPONSE
Definition: roseus.cpp:152
RoseusStaticData::~RoseusStaticData
~RoseusStaticData()
Definition: roseus.cpp:130
ROSEUS
pointer ROSEUS(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:572
K_ROSEUS_DESERIALIZE
pointer K_ROSEUS_DESERIALIZE
Definition: roseus.cpp:152
EuslispMessage::__getServerMD5Sum
virtual const string __getServerMD5Sum() const
Definition: roseus.cpp:252
master.h
ros::Duration
K_ROSEUS_LAST_REAL
pointer K_ROSEUS_LAST_REAL
Definition: roseus.cpp:152
param.h
K_FUNCTION_DOCUMENTATION
pointer K_FUNCTION_DOCUMENTATION
ros::init_options::NoSigintHandler
NoSigintHandler
K_ROSEUS_CURRENT_REAL
pointer K_ROSEUS_CURRENT_REAL
Definition: roseus.cpp:152
QROSINFO
pointer QROSINFO
Definition: roseus.cpp:152
ros::serialization::deserializeMessage
void deserializeMessage(const SerializedMessage &m, M &message)
EuslispMessage::__getMessageDefinition
virtual const string __getMessageDefinition() const
Definition: roseus.cpp:246
K_ROSEUS_ONESHOT
pointer K_ROSEUS_ONESHOT
Definition: roseus.cpp:152
ros::SerializedMessage::buf
boost::shared_array< uint8_t > buf
DurationBase< WallDuration >::sec
int32_t sec
K_ROSEUS_GET
pointer K_ROSEUS_GET
Definition: roseus.cpp:152
ROSEUS_SPINONCE
pointer ROSEUS_SPINONCE(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:719
getInteger
int getInteger(pointer message, pointer method)
Definition: roseus.cpp:188
XmlRpc::XmlRpcValue
def_rosconsole_formatter
#define def_rosconsole_formatter(funcname, rosfuncname)
Definition: roseus.cpp:798
v
GLfloat v[8][3]
ros::serialization::Serializer
ros::NodeHandle
ros::SubscriptionCallbackHelperCallParams
ckarg
ckarg(2)
ROSEUS_SET_LOGGER_LEVEL
pointer ROSEUS_SET_LOGGER_LEVEL(register context *ctx, int n, pointer *argv)
Definition: roseus.cpp:1748
cell::cellunion::str
struct string str
s_mapTimered
#define s_mapTimered
Definition: roseus.cpp:149
ros::ServiceCallbackHelperCallParams::connection_header
boost::shared_ptr< M_string > connection_header
ros::master::TopicInfo::name
std::string name
thr_self
unsigned int thr_self()
ros::M_string
std::map< std::string, std::string > M_string
context::intsig
int intsig
ros::Time::now
static Time now()
EuslispMessage::EuslispMessage
EuslispMessage(const EuslispMessage &r)
Definition: roseus.cpp:218
callframe::form
pointer form
QANON
pointer QANON
Definition: roseus.cpp:152


roseus
Author(s): Kei Okada
autogenerated on Fri Feb 3 2023 03:35:33