eustf.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, JSK Lab, the Univ. of Tokyo
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: Ryohei Ueda
36 #include <stdio.h>
37 #include <stdlib.h>
38 #include <unistd.h>
39 #include <string.h>
40 #include <signal.h>
41 #include <math.h>
42 #include <time.h>
43 #include <pthread.h>
44 #include <setjmp.h>
45 #include <errno.h>
46 
47 #include <list>
48 #include <vector>
49 #include <set>
50 #include <string>
51 #include <map>
52 #include <sstream>
53 
54 #include <cstdio>
55 #include <boost/thread/mutex.hpp>
56 #include <boost/thread/condition.hpp>
57 #include <boost/shared_ptr.hpp>
58 
59 #include <ros/init.h>
60 #include <ros/time.h>
61 #include <ros/rate.h>
62 #include <ros/master.h>
63 #include <ros/this_node.h>
64 #include <ros/node_handle.h>
65 #include <ros/service.h>
66 #include <tf/tf.h>
67 #include <tf/transform_listener.h>
68 #include <tf/transform_datatypes.h>
70 
71 #include <tf2_ros/buffer_client.h>
72 #ifdef TF2_ROS_VERSION_3 // if this is groovy
73 #define tf2_ros tf2
74 #endif
75 
76 // for eus.h
77 #define class eus_class
78 #define throw eus_throw
79 #define export eus_export
80 #define vector eus_vector
81 #define string eus_string
82 #ifdef __clang__
83 #undef MAX
84 #undef MIN
85 #endif
86 #include "eus.h"
87 
88 extern "C" {
89  pointer ___eustf(register context *ctx, int n, pointer *argv, pointer env);
91  char modname[] = "___eustf";
92  return add_module_initializer(modname, (pointer (*)())___eustf);}
93 }
94 
95 #undef class
96 #undef throw
97 #undef export
98 #undef vector
99 #undef string
100 
101 using namespace ros;
102 using namespace std;
103 
104 
105 /***********************************************************
106  * TF wrapper
107  ************************************************************/
108 
109 #define set_ros_time(time,argv) \
110  if (isvector(argv) && (elmtypeof(argv)==ELM_INT)) { \
111  time.sec = argv->c.ivec.iv[0]; \
112  time.nsec = argv->c.ivec.iv[1]; \
113  } else { \
114  error(E_NOVECTOR); \
115  }
116 
117 /* */
119 {
120  numunion nu;
121  ckarg(2);
122  bool interpolating = ((argv[0]==T)?true:false);
123  float cache_time = ckfltval(argv[1]);
124  return(makeint((eusinteger_t)(new tf::Transformer(interpolating, ros::Duration(cache_time)))));
125 }
126 
128 {
129  ckarg(1);
130  tf::Transformer *tf = (tf::Transformer *)(intval(argv[0]));
131  string str = tf->allFramesAsString();
132  return(makestring((char *)(str.c_str()), str.length()));
133 }
134 
136 {
137  ckarg(7);
138  tf::Transformer *tf = (tf::Transformer *)(intval(argv[0]));
139  if (!isvector(argv[1])) error(E_NOVECTOR);
140  if (!isvector(argv[2])) error(E_NOVECTOR);
141  eusfloat_t *pos = argv[1]->c.fvec.fv;
142  eusfloat_t *rot = argv[2]->c.fvec.fv;
143  isintvector(argv[3]);
144  eusinteger_t *stamp = argv[3]->c.ivec.iv;
145  if (!isstring(argv[4])) error(E_NOSTRING);
146  if (!isstring(argv[5])) error(E_NOSTRING);
147  if (!isstring(argv[6])) error(E_NOSTRING);
148  std::string frame_id = std::string((char*)(argv[4]->c.str.chars));
149  std::string child_frame_id = std::string((char*)(argv[5]->c.str.chars));
150  std::string authority= std::string((char*)(argv[6]->c.str.chars));
151  tf::StampedTransform transform;
152  transform.setOrigin(tf::Vector3(pos[0], pos[1], pos[2]));
153  transform.setRotation(tf::Quaternion(rot[3], rot[0], rot[1], rot[2]));
154  transform.frame_id_ = frame_id;
155  transform.child_frame_id_ = child_frame_id;
156  transform.stamp_.sec = stamp[0];
157  transform.stamp_.nsec = stamp[1];
158  bool ret = tf->setTransform(transform, authority);
159  return(ret?T:NIL);
160 }
161 
163 {
164  numunion nu;
165  ckarg(6);
167  std::string target_frame, source_frame;
168  ros::Time time;
169  float timeout = 0, duration = 0;
170  bool ret;
171 
172  tf = (tf::Transformer *)(intval(argv[0]));
173  if (isstring(argv[1]))
174  target_frame = std::string((char*)(argv[1]->c.str.chars));
175  else error(E_NOSTRING);
176 
177  if (isstring(argv[2]))
178  source_frame = std::string((char*)(argv[2]->c.str.chars));
179  else error(E_NOSTRING);
180 
181  set_ros_time(time,argv[3]);
182 
183  if (isint(argv[4])) timeout = (float)intval(argv[4]);
184  else if (isflt(argv[4])) timeout = (float)fltval(argv[4]);
185  else error(E_NONUMBER);
186 
187  if (isint(argv[5])) duration = (float)intval(argv[5]);
188  else if (isflt(argv[5])) duration = (float)fltval(argv[5]);
189  else error(E_NONUMBER);
190 
191  std::string err_str = std::string();
192  ret = tf->waitForTransform(target_frame, source_frame, time,
193  ros::Duration(timeout), ros::Duration(duration),
194  &err_str);
195  if(!ret) {
196  ROS_WARN_STREAM("waitForTransform failed! : " << err_str);
197  }
198  ROS_DEBUG_STREAM("waitForTransform : "
199  << "target_frame : " << target_frame
200  << "source_frame : " << source_frame
201  << "time : " << time
202  << "timeout : " << timeout
203  << "duration : " << duration
204  << "return : " << ret);
205 
206  return((ret==true)?(T):(NIL));
207 }
208 
210 {
211  numunion nu;
212  ckarg(8);
214  std::string target_frame, source_frame, fixed_frame;
215  ros::Time target_time, source_time;
216  float timeout = 0, duration = 0;
217  bool ret;
218 
219  tf = (tf::Transformer *)(intval(argv[0]));
220  if (isstring(argv[1]))
221  target_frame = std::string((char*)(argv[1]->c.str.chars));
222  else error(E_NOSTRING);
223 
224  set_ros_time(target_time,argv[2]);
225 
226  if (isstring(argv[3]))
227  source_frame = std::string((char*)(argv[3]->c.str.chars));
228  else error(E_NOSTRING);
229 
230  set_ros_time(source_time,argv[4]);
231 
232  if (isstring(argv[5]))
233  fixed_frame = std::string((char*)(argv[5]->c.str.chars));
234  else error(E_NOSTRING);
235 
236  if (isint(argv[6])) timeout = (float)intval(argv[6]);
237  else if (isflt(argv[6])) timeout = (float)fltval(argv[6]);
238  else error(E_NONUMBER);
239 
240  if (isint(argv[7])) duration = (float)intval(argv[7]);
241  else if (isflt(argv[7])) duration = (float)fltval(argv[7]);
242  else error(E_NONUMBER);
243 
244  std::string err_str = std::string();
245  ret = tf->waitForTransform(target_frame, target_time,
246  source_frame, source_time,
247  fixed_frame,
248  ros::Duration(timeout), ros::Duration(duration),
249  &err_str);
250  if(!ret) {
251  ROS_WARN_STREAM("waitForTransformFull failed! : " << err_str);
252  }
253  ROS_DEBUG_STREAM("waitForTransformFull : "
254  << "target_frame : " << target_frame
255  << "target_time : " << target_time
256  << "source_frame : " << source_frame
257  << "source_time : " << source_time
258  << "fixed_frame : " << fixed_frame
259  << "timeout : " << timeout
260  << "duration : " << duration
261  << "return : " << ret);
262 
263  return((ret==true)?(T):(NIL));
264 }
265 
267 {
268  ckarg(4);
270  std::string target_frame, source_frame;
271  ros::Time time;
272  bool ret;
273 
274  tf = (tf::Transformer *)(intval(argv[0]));
275  if (isstring(argv[1]))
276  target_frame = std::string((char*)(argv[1]->c.str.chars));
277  else error(E_NOSTRING);
278 
279  if (isstring(argv[2]))
280  source_frame = std::string((char*)(argv[2]->c.str.chars));
281  else error(E_NOSTRING);
282 
283  set_ros_time(time,argv[3]);
284 
285  std::string err_str = std::string();
286  ret = tf->canTransform(target_frame, source_frame, time, &err_str);
287  if(!ret) {
288  ROS_WARN_STREAM("canTransform " << target_frame << " " << source_frame << " failed! : " << err_str);
289  }
290  ROS_DEBUG_STREAM("canTransform : "
291  << "target_frame : " << target_frame
292  << "source_frame : " << source_frame
293  << "time : " << time
294  << "return : " << ret);
295 
296  return((ret==true)?(T):(NIL));
297 }
298 
300 {
301  ckarg(7);
303  std::string target_frame, source_frame, fixed_frame;
304  ros::Time target_time, source_time;
305  bool ret;
306 
307  tf = (tf::Transformer *)(intval(argv[0]));
308  if (isstring(argv[1]))
309  target_frame = std::string((char*)(argv[1]->c.str.chars));
310  else error(E_NOSTRING);
311 
312  set_ros_time(target_time,argv[3]);
313 
314  if (isstring(argv[3]))
315  source_frame = std::string((char*)(argv[3]->c.str.chars));
316  else error(E_NOSTRING);
317 
318  set_ros_time(source_time,argv[4]);
319 
320  if (isstring(argv[5]))
321  fixed_frame = std::string((char*)(argv[5]->c.str.chars));
322  else error(E_NOSTRING);
323 
324  std::string err_str = std::string();
325  ret = tf->canTransform(target_frame, target_time,
326  source_frame, source_time,
327  fixed_frame, &err_str);
328  if(!ret) {
329  ROS_WARN_STREAM("canTransformFull " << target_frame << " " << source_frame << " failed! : " << err_str);
330  }
331  ROS_DEBUG_STREAM("canTransformFull : "
332  << "target_frame : " << target_frame
333  << "target_time : " << target_time
334  << "source_frame : " << source_frame
335  << "source_time : " << source_time
336  << "fixed_frame : " << fixed_frame
337  << "return : " << ret);
338 
339  return((ret==true)?(T):(NIL));
340 }
341 
343 {
344  ROS_ERROR("%s is not implemented yet", __PRETTY_FUNCTION__);
345  return(T);
346 }
347 
349 {
350  ckarg(1);
351  tf::Transformer *tf = (tf::Transformer *)(intval(argv[0]));
352  tf->clear();
353  return(T);
354 }
355 
357 {
358  ckarg(2);
360  std::string frame_id;
361 
362  tf = (tf::Transformer *)(intval(argv[0]));
363  if (!isstring(argv[1])) error(E_NOSTRING);
364  frame_id = std::string((char*)(argv[1]->c.str.chars));
365  return(tf->frameExists(frame_id)?T:NIL);
366 }
367 
369 {
370  ckarg(1);
371  tf::Transformer *tf = (tf::Transformer *)(intval(argv[0]));
372  std::vector< std::string > ids;
373  pointer str = NIL;
374 
375  tf->getFrameStrings(ids);
376  for (std::vector< std::string >::iterator s = ids.begin(); s != ids.end(); s++) {
377  str=cons(ctx,makestring((char *)(s->c_str()),s->length()),str);
378  }
379 
380  return(str);
381 }
382 
384 {
385  ckarg(3);
387  std::string source_frame, target_frame, error_string;
388 
389  tf = (tf::Transformer *)(intval(argv[0]));
390  if (!isstring(argv[1])) error(E_NOSTRING);
391  source_frame = std::string((char*)(argv[1]->c.str.chars));
392  if (!isstring(argv[2])) error(E_NOSTRING);
393  target_frame = std::string((char*)(argv[2]->c.str.chars));
394 
395  ros::Time time;
396  int r = tf->getLatestCommonTime(source_frame, target_frame, time, &error_string);
397  if ( r == 0 ) {
398  return(cons(ctx,makeint(time.sec),makeint(time.nsec)));
399  } else {
400  ROS_ERROR_STREAM("getLatestCommonTime " << target_frame << " " << source_frame << " failed! : " << error_string);
401  return(NIL);
402  }
403 }
404 
406 {
407  ckarg(4);
409  std::string target_frame, source_frame;
410  ros::Time time;
411 
412  tf = (tf::Transformer *)(intval(argv[0]));
413  if (!isstring(argv[1])) error(E_NOSTRING);
414  target_frame = std::string((char*)(argv[1]->c.str.chars));
415  if (!isstring(argv[2])) error(E_NOSTRING);
416  source_frame = std::string((char*)(argv[2]->c.str.chars));
417 
418  set_ros_time(time,argv[3]);
419 
420  tf::StampedTransform transform;
421  try {
422  tf->lookupTransform(target_frame, source_frame, time, transform);
423  } catch ( std::runtime_error e ) {
424  ROS_ERROR("%s",e.what()); return(NIL);
425  }
426 
427  pointer vs = makefvector(7); //pos[3] + rot[4](angle-axis quaternion)
428  vpush(vs);
429  tf::Vector3 p = transform.getOrigin();
430  tf::Quaternion q = transform.getRotation();
431  vs->c.fvec.fv[0] = p.getX();
432  vs->c.fvec.fv[1] = p.getY();
433  vs->c.fvec.fv[2] = p.getZ();
434  vs->c.fvec.fv[3] = q.getW();
435  vs->c.fvec.fv[4] = q.getX();
436  vs->c.fvec.fv[5] = q.getY();
437  vs->c.fvec.fv[6] = q.getZ();
438  vpop();
439  return(vs);
440 }
441 
443 {
444  ckarg(6);
446  std::string target_frame, source_frame, fixed_frame;
447  ros::Time target_time, source_time;
448 
449  tf = (tf::Transformer *)(intval(argv[0]));
450  if (!isstring(argv[1])) error(E_NOSTRING);
451  target_frame = std::string((char*)(argv[1]->c.str.chars));
452 
453  set_ros_time(target_time,argv[2]);
454 
455  if (!isstring(argv[3])) error(E_NOSTRING);
456  source_frame = std::string((char*)(argv[3]->c.str.chars));
457 
458  set_ros_time(source_time,argv[4]);
459 
460  if (!isstring(argv[5])) error(E_NOSTRING);
461  fixed_frame = std::string((char*)(argv[5]->c.str.chars));
462 
463  tf::StampedTransform transform;
464  try {
465  tf->lookupTransform(target_frame, target_time,
466  source_frame, source_time, fixed_frame, transform);
467  } catch ( std::runtime_error e ) {
468  ROS_ERROR("%s",e.what()); return(NIL);
469  }
470 
471  pointer vs = makefvector(7); //pos[3] + rot[4](angle-axis quaternion)
472  vpush(vs);
473  tf::Vector3 p = transform.getOrigin();
474  tf::Quaternion q = transform.getRotation();
475  vs->c.fvec.fv[0] = p.getX();
476  vs->c.fvec.fv[1] = p.getY();
477  vs->c.fvec.fv[2] = p.getZ();
478  vs->c.fvec.fv[3] = q.getW();
479  vs->c.fvec.fv[4] = q.getX();
480  vs->c.fvec.fv[5] = q.getY();
481  vs->c.fvec.fv[6] = q.getZ();
482  vpop();
483  return(vs);
484 }
485 
486 
488 {
489  ckarg(5); // tf, target_frame, time, frame_id,
490  // pose as float-vector. its vector is a vector
491  // appended position and angle-vector quaternion
493  if (!isstring(argv[1])) error(E_NOSTRING);
494  std::string target_frame = std::string((char*)(argv[1]->c.str.chars));
495  ros::Time tm;
496  set_ros_time(tm, argv[2]);
497 
498  if (!isstring(argv[3])) error(E_NOSTRING);
499  std::string frame_id = std::string((char*)(argv[3]->c.str.chars));
500 
501  geometry_msgs::PoseStamped input, output;
502  // setup input
503  input.pose.position.x = argv[4]->c.fvec.fv[0];
504  input.pose.position.y = argv[4]->c.fvec.fv[1];
505  input.pose.position.z = argv[4]->c.fvec.fv[2];
506  input.pose.orientation.w = argv[4]->c.fvec.fv[3]; // angle-vector format
507  input.pose.orientation.x = argv[4]->c.fvec.fv[4];
508  input.pose.orientation.y = argv[4]->c.fvec.fv[5];
509  input.pose.orientation.z = argv[4]->c.fvec.fv[6];
510  // input.header.
511  input.header.stamp = tm;
512  input.header.frame_id = frame_id;
513 
514  try {
515  tf->transformPose(target_frame, input, output);
516  } catch ( std::runtime_error e ) {
517  ROS_ERROR("%s",e.what()); return(NIL);
518  }
519 
520  pointer vs = makefvector(7); //pos[3] + rot[4](angle-axis quaternion)
521  vpush(vs);
522  vs->c.fvec.fv[0] = output.pose.position.x;
523  vs->c.fvec.fv[1] = output.pose.position.y;
524  vs->c.fvec.fv[2] = output.pose.position.z;
525  vs->c.fvec.fv[3] = output.pose.orientation.w;
526  vs->c.fvec.fv[4] = output.pose.orientation.x;
527  vs->c.fvec.fv[5] = output.pose.orientation.y;
528  vs->c.fvec.fv[6] = output.pose.orientation.z;
529  vpop();
530  return(vs);
531 }
532 
534 {
535  numunion nu;
536  ckarg(4);
538  std::string reference_frame, moving_frame;
539  float time = 0, duration = 0;
540 
541  tf = (tf::Transformer *)(intval(argv[0]));
542  if (!isstring(argv[1])) error(E_NOSTRING);
543  reference_frame = std::string((char*)(argv[1]->c.str.chars));
544 
545  if (!isstring(argv[2])) error(E_NOSTRING);
546  moving_frame = std::string((char*)(argv[2]->c.str.chars));
547 
548  if (isint(argv[3])) time = (float)intval(argv[3]);
549  else if (isflt(argv[3])) time = (float)fltval(argv[3]);
550  else error(E_NONUMBER);
551 
552  if (isint(argv[4])) duration = (float)intval(argv[4]);
553  else if (isflt(argv[4])) duration = (float)fltval(argv[4]);
554  else error(E_NONUMBER);
555 
556  geometry_msgs::Twist velocity;
557  // ROS_ERROR("%s is not implemented yet since lookupVelocity seems obsoluted", __PRETTY_FUNCTION__);
558  tf->lookupTwist(reference_frame, moving_frame, ros::Time(time), ros::Duration(duration), velocity);
559 
560  pointer vs = makefvector(6); //pos[3] + rot[3](angle-axis)
561  vpush(vs);
562  vs->c.fvec.fv[0] = velocity.linear.x;
563  vs->c.fvec.fv[1] = velocity.linear.y;
564  vs->c.fvec.fv[2] = velocity.linear.z;
565  vs->c.fvec.fv[3] = velocity.angular.x;
566  vs->c.fvec.fv[4] = velocity.angular.y;
567  vs->c.fvec.fv[5] = velocity.angular.z;
568  vpop();
569  return(vs);
570 }
571 
572 /* */
574 {
575  if( ! ros::ok() ) { error(E_USER,"You must call ros::init() before creating the first NodeHandle"); }
576  numunion nu;
577  ckarg(2);
578  float cache_time = ckfltval(argv[0]);
579  bool spin_thread = ((argv[1]==T)?true:false);
580  return(makeint((eusinteger_t)(new tf::TransformListener(ros::Duration(cache_time), spin_thread))));
581 }
582 
584 {
585  ckarg(1);
587  delete(tf);
588  return(T);
589 }
590 
591 /* */
593 {
594  numunion nu;
595  ckarg(2);
597  tf = (tf::Transformer *)(intval(argv[0]));
598  float distance = ckfltval(argv[1]);
599 
600  tf->setExtrapolationLimit(ros::Duration(distance));
601  return(T);
602 }
603 
605 {
606  ckarg(3);
608  std::string frame_id, parent;
609  ros::Time time;
610 
611  tf = (tf::Transformer *)(intval(argv[0]));
612 
613  if (isstring(argv[1]))
614  frame_id = std::string((char*)(argv[1]->c.str.chars));
615  else error(E_NOSTRING);
616 
617  set_ros_time(time,argv[2]);
618 
619  try {
620  bool ret = tf->getParent(frame_id, time, parent);
621  return(ret?makestring((char *)parent.c_str(),parent.length()):NIL);
622  } catch ( std::runtime_error e ) {
623  ROS_ERROR("%s",e.what()); return(NIL);
624  }
625 }
626 
627 /* */
629 {
630  if( ! ros::ok() ) { error(E_USER,"You must call ros::init() before creating the first NodeHandle"); }
632 }
633 
635 
636  /* ptr pos quarternion parent_frame_id, child_frame_id, time */
637  ckarg(6);
638 
640 
641  isintvector(argv[5]);
642  ros::Time tm;
643  tm.sec = argv[5]->c.ivec.iv[0];
644  tm.nsec = argv[5]->c.ivec.iv[1];
645 
646  eusfloat_t *pos, *quaternion;
647  std::string p_frame_id, c_frame_id;
648  isfltvector(argv[1]);
649  isfltvector(argv[2]);
650  isstring(argv[3]);
651  isstring(argv[4]);
652  pos = argv[1]->c.fvec.fv;
653  quaternion= argv[2]->c.fvec.fv;
654  p_frame_id = (char *)argv[3]->c.str.chars;
655  c_frame_id = (char *)argv[4]->c.str.chars;
656 
657  geometry_msgs::TransformStamped trans;
658  trans.header.stamp = tm;
659  trans.header.frame_id = p_frame_id;
660  trans.child_frame_id = c_frame_id;
661  trans.transform.translation.x = pos[0]/1000.0;
662  trans.transform.translation.y = pos[1]/1000.0;
663  trans.transform.translation.z = pos[2]/1000.0;
664 
665  trans.transform.rotation.w = quaternion[0];
666  trans.transform.rotation.x = quaternion[1];
667  trans.transform.rotation.y = quaternion[2];
668  trans.transform.rotation.z = quaternion[3];
669 
670  bc->sendTransform(trans);
671 
672  return (T);
673 }
674 
675 /* tf2 */
677 {
678  if(!ros::ok()) { error(E_USER, "You must call (ros::roseus \"nodename\") before creating the first NodeHandle"); }
679  /* &optional (ns "tf2_buffer_server") (check_frequency 10.0) (timeout_padding 2.0) */
680  numunion nu;
681  std::string ns_str ("tf2_buffer_server");
682  double check_frequency = 10.0;
683  ros::Duration timeout_padding(2.0);
684 
685  ckarg2(0, 3);
686  if (n > 0) {
687  if (isstring (argv[0])) {
688  ns_str.assign ((char *)(argv[0]->c.str.chars));
689  } else {
690  error(E_NOSTRING);
691  }
692  }
693  if (n > 1) {
694  check_frequency = ckfltval(argv[1]);
695  }
696  if (n > 2) {
697  eusfloat_t pd = ckfltval(argv[2]);
698  timeout_padding = ros::Duration(pd);
699  }
700 
701  return(makeint((eusinteger_t)(new tf2_ros::BufferClient (ns_str, check_frequency, timeout_padding))));
702 }
703 
705 {
706  ckarg(1);
707  tf2_ros::BufferClient *tfbc = (tf2_ros::BufferClient *)(intval(argv[0]));
708  delete(tfbc);
709  return(T);
710 }
711 
713 {
714  ckarg2(1, 2);
715  tf2_ros::BufferClient *tfbc = (tf2_ros::BufferClient *)(intval(argv[0]));
716  numunion nu;
717  bool ret;
718  if (n > 1) {
719  ros::Duration tm(ckfltval(argv[1]));
720  ret = tfbc->waitForServer(tm);
721  } else {
722  ret = tfbc->waitForServer();
723  }
724  return((ret==true)?(T):(NIL));
725 }
726 
728 {
729  ckarg2(4, 5);
730  tf2_ros::BufferClient *tfbc = (tf2_ros::BufferClient *)(intval(argv[0]));
731  numunion nu;
732  std::string target_frame, source_frame;
733  ros::Time time;
734  ros::Duration timeout(0.0);
735  bool ret;
736 
737  if (isstring(argv[1])) {
738  char *cstr = (char*)(argv[1]->c.str.chars);
739  if (cstr[0] == '/') {
740  target_frame.assign((char *)(cstr+1));
741  } else {
742  target_frame.assign(cstr);
743  }
744  } else error(E_NOSTRING);
745 
746  if (isstring(argv[2])) {
747  char *cstr = (char*)(argv[2]->c.str.chars);
748  if (cstr[0] == '/') {
749  source_frame.assign((char *)(cstr+1));
750  } else {
751  source_frame.assign(cstr);
752  }
753  } else error(E_NOSTRING);
754 
755  set_ros_time(time, argv[3]);
756 
757  if (n > 4) {
758  timeout = ros::Duration(ckfltval(argv[4]));
759  }
760  //target_frame.
761  std::string err_str = std::string();
762  ret = tfbc->canTransform(target_frame, source_frame, time, timeout, &err_str);
763  if(!ret) {
764  ROS_WARN_STREAM("BufferClient::waitForTransform failed! : " << err_str);
765  }
766  ROS_DEBUG_STREAM("BufferClient::waitForTransform : "
767  << "target_frame : " << target_frame
768  << "source_frame : " << source_frame
769  << "time : " << time
770  << "timeout : " << timeout
771  << "return : " << ret);
772 
773  return((ret==true)?(T):(NIL));
774 }
775 
777 {
778  ckarg2(4, 5);
779  tf2_ros::BufferClient *tfbc = (tf2_ros::BufferClient *)(intval(argv[0]));
780  numunion nu;
781  std::string target_frame, source_frame;
782  ros::Time time;
783  ros::Duration timeout(0.0);
784 
785  if (!isstring(argv[1])) error(E_NOSTRING);
786  target_frame = std::string((char*)(argv[1]->c.str.chars));
787 
788  if (!isstring(argv[2])) error(E_NOSTRING);
789  source_frame = std::string((char*)(argv[2]->c.str.chars));
790 
791  set_ros_time(time, argv[3]);
792 
793  if (n > 4) {
794  timeout = ros::Duration(ckfltval(argv[4]));
795  }
796 
797  geometry_msgs::TransformStamped trans;
798  try {
799  trans = tfbc->lookupTransform(target_frame, source_frame, time, timeout);
800  } catch (std::runtime_error e) {
801  ROS_ERROR("%s", e.what()); return(NIL);
802  }
803 
804  pointer vs = makefvector(7); //pos[3] + rot[4](angle-axis quaternion)
805  vpush(vs);
806  //trans.header.frame_id
807  //trans.child_frame_id
808  //trans.header.stamp --> ??
809  vs->c.fvec.fv[0] = trans.transform.translation.x;
810  vs->c.fvec.fv[1] = trans.transform.translation.y;
811  vs->c.fvec.fv[2] = trans.transform.translation.z;
812  vs->c.fvec.fv[3] = trans.transform.rotation.w;
813  vs->c.fvec.fv[4] = trans.transform.rotation.x;
814  vs->c.fvec.fv[5] = trans.transform.rotation.y;
815  vs->c.fvec.fv[6] = trans.transform.rotation.z;
816  vpop();
817 
818  return(vs);
819 }
820 
821 #include "defun.h"
822 pointer ___eustf(register context *ctx, int n, pointer *argv, pointer env)
823 {
824  pointer rospkg,p=Spevalof(PACKAGE);
825  rospkg=findpkg(makestring("TF",2));
826  if (rospkg == 0) rospkg=makepkg(ctx,makestring("TF", 2),NIL,NIL);
827  Spevalof(PACKAGE)=rospkg;
828 
829  rospkg=findpkg(makestring("ROS",3));
830  if (rospkg == 0 ) {
831  ROS_ERROR("Coudld not found ROS package; Please load eusros.so");
832  exit(2);
833  }
834  Spevalof(PACKAGE)=rospkg;
835  defun(ctx,"EUSTF-TRANSFORMER",argv[0],(pointer (*)())EUSTF_TRANSFORMER,"A Class which provides coordinate transforms between any two frames in a system");
836  defun(ctx,"EUSTF-ALL-FRAMES-AS-STRING",argv[0],(pointer (*)())EUSTF_ALLFRAMESASSTRING,"A way to see what frames have been cached Useful for debugging");
837  defun(ctx,"EUSTF-SET-TRANSFORM",argv[0],(pointer (*)())EUSTF_SETTRANSFORM,"Add transform information to the tf data structure");
838  defun(ctx,"EUSTF-WAIT-FOR-TRANSFORM",argv[0],(pointer (*)())EUSTF_WAITFORTRANSFORM,"Block until a transform is possible or it times out");
839  defun(ctx,"EUSTF-WAIT-FOR-TRANSFORM-FULL",argv[0],(pointer (*)())EUSTF_WAITFORTRANSFORMFULL,"Block until a transform is possible or it times out");
840  defun(ctx,"EUSTF-CAN-TRANSFORM",argv[0],(pointer (*)())EUSTF_CANTRANSFORM,"Test if a transform is possible");
841  defun(ctx,"EUSTF-CAN-TRANSFORM-FULL",argv[0],(pointer (*)())EUSTF_CANTRANSFORMFULL,"Test if a transform is possible");
842  defun(ctx,"EUSTF-CHAIN",argv[0],(pointer (*)())EUSTF_CHAIN,"Debugging function that will print the spanning chain of transforms");
843  defun(ctx,"EUSTF-CLEAR",argv[0],(pointer (*)())EUSTF_CLEAR,"Clear all data");
844  defun(ctx,"EUSTF-FRAME-EXISTS",argv[0],(pointer (*)())EUSTF_FRAMEEXISTS,"Check if a frame exists in the tree");
845  defun(ctx,"EUSTF-GET-FRAME-STRINGS",argv[0],(pointer (*)())EUSTF_GETFRAMESTRINGS,"A way to get a std::vector of available frame ids");
846  defun(ctx,"EUSTF-GET-LATEST-COMMON-TIME",argv[0],(pointer (*)())EUSTF_GETLATERSTCOMMONTIME,"Return the latest rostime which is common across the spanning set zero if fails to cross");
847  defun(ctx,"EUSTF-LOOKUP-TRANSFORM",argv[0],(pointer (*)())EUSTF_LOOKUPTRANSFORM,"Get the transform between two frames by frame ID");
848  defun(ctx,"EUSTF-LOOKUP-TRANSFORM-FULL",argv[0],(pointer (*)())EUSTF_LOOKUPTRANSFORMFULL,"Get the transform between two frames by frame ID");
849  defun(ctx,"EUSTF-TRANSFORM-POSE",argv[0],(pointer (*)())EUSTF_TRANSFORMPOSE,"Transform a Stamped Pose into the target frame This can throw anything a lookupTransform can throw as well as tf::InvalidArgument");
850  defun(ctx,"EUSTF-LOOKUP-VELOCITY",argv[0],(pointer (*)())EUSTF_LOOKUPVELOCITY,"Lookup the twist of the tracking_frame with respect to the observation frame in the reference_frame using the reference point");
851  /* */
852  defun(ctx,"EUSTF-TRANSFORM-LISTENER",argv[0],(pointer (*)())EUSTF_TRANSFORM_LISTENER,"This class inherits from Transformer and automatically subscribes to ROS transform messages");
853  defun(ctx,"EUSTF-TRANSFORM-LISTENER-DISPOSE",argv[0],(pointer (*)())EUSTF_TRANSFORM_LISTENER_DISPOSE,"Destructor for TransformListener");
854 
855  /* */
856  defun(ctx,"EUSTF-SET-EXTRAPOLATION-LIMIT",argv[0],(pointer (*)())EUSTF_SETEXTRAPOLATIONLIMIT,"Set the distance which tf is allow to extrapolate");
857  defun(ctx,"EUSTF-GET-PARENT",argv[0],(pointer (*)())EUSTF_GETPARENT,"Fill the parent of a frame");
858  /* */
859  defun(ctx,"EUSTF-TRANSFORM-BROADCASTER",argv[0],(pointer (*)())EUSTF_TRANSFORM_BROADCASTER,"This class provides an easy way to publish coordinate frame transform information. It will handle all the messaging and stuffing of messages. And the function prototypes lay out all the necessary data needed for each message");
860  defun(ctx,"EUSTF-SEND-TRANSFORM",argv[0],(pointer (*)())EUSTF_SEND_TRANSFORM,"Send a StampedTransform The stamped data structure includes frame_id, and time, and parent_id already");
861 
862  /* tf2 */
863  defun(ctx,"EUSTF-BUFFER-CLIENT",argv[0],(pointer (*)())EUSTF_BUFFER_CLIENT,"Action client-based implementation of the tf2_ros::BufferInterface abstract data type. BufferClient uses actionlib to coordinate waiting for available transforms.");
864  defun(ctx,"EUSTF-BUFFER-CLIENT-DISPOSE",argv[0],(pointer (*)())EUSTF_BUFFER_CLIENT_DISPOSE,"tf2 BufferClient destructor");
865  defun(ctx,"EUSTF-TF2-WAIT-FOR-SERVER",argv[0],(pointer (*)())EUSTF_TFBC_WAITFORSERVER,"Block until the action server is ready to respond to requests");
866  defun(ctx,"EUSTF-TF2-CAN-TRANSFORM",argv[0],(pointer (*)())EUSTF_TFBC_CANTRANSFORM,"Test if a transform is possible");
867  defun(ctx,"EUSTF-TF2-LOOKUP-TRANSFORM",argv[0],(pointer (*)())EUSTF_TFBC_LOOKUPTRANSFORM,"Get the transform between two frames by frame ID");
868 
869  pointer_update(Spevalof(PACKAGE),p);
870  return 0;
871 }
TFSIMD_FORCE_INLINE void setRotation(const Quaternion &q)
pointer makeint(eusinteger_t v)
void getFrameStrings(std::vector< std::string > &ids) const
pointer EUSTF_BUFFER_CLIENT_DISPOSE(register context *ctx, int n, pointer *argv)
Definition: eustf.cpp:704
struct string str
byte chars[1]
pointer EUSTF_TRANSFORM_LISTENER(register context *ctx, int n, pointer *argv)
Definition: eustf.cpp:573
pointer T
bool getParent(const std::string &frame_id, ros::Time time, std::string &parent) const
pointer EUSTF_LOOKUPTRANSFORM(register context *ctx, int n, pointer *argv)
Definition: eustf.cpp:405
float fv[1]
pointer makepkg(context *, pointer, pointer, pointer)
int getLatestCommonTime(const std::string &source_frame, const std::string &target_frame, ros::Time &time, std::string *error_string) const
pointer EUSTF_WAITFORTRANSFORMFULL(register context *ctx, int n, pointer *argv)
Definition: eustf.cpp:209
pointer ___eustf(register context *ctx, int n, pointer *argv, pointer env)
Definition: eustf.cpp:822
pointer makefvector(int)
bool setTransform(const StampedTransform &transform, const std::string &authority="default_authority")
void register_eustf()
Definition: eustf.cpp:90
TFSIMD_FORCE_INLINE const tfScalar & getY() const
std::string allFramesAsString() const
TFSIMD_FORCE_INLINE tfScalar distance(const Vector3 &v) const
TFSIMD_FORCE_INLINE const tfScalar & getW() const
ROS_INFO ROS_ERROR int n
Definition: roseus.cpp:818
E_USER
TFSIMD_FORCE_INLINE const tfScalar & getZ() const
ckarg(2)
void add_module_initializer(char *, pointer(*)())
pointer EUSTF_TRANSFORMER(register context *ctx, int n, pointer *argv)
Definition: eustf.cpp:118
float ckfltval()
bool canTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, std::string *error_msg=NULL) const
pointer EUSTF_ALLFRAMESASSTRING(register context *ctx, int n, pointer *argv)
Definition: eustf.cpp:127
bool waitForTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration &timeout, const ros::Duration &polling_sleep_duration=ros::Duration(0.01), std::string *error_msg=NULL) const
struct intvector ivec
pointer EUSTF_CHAIN(register context *ctx, int n, pointer *argv)
Definition: eustf.cpp:342
pointer EUSTF_LOOKUPVELOCITY(register context *ctx, int n, pointer *argv)
Definition: eustf.cpp:533
#define string
Definition: eustf.cpp:81
bool waitForServer(const ros::Duration &timeout=ros::Duration(0))
union cell::cellunion c
pointer EUSTF_TRANSFORMPOSE(register context *ctx, int n, pointer *argv)
Definition: eustf.cpp:487
std::string child_frame_id_
ROS_INFO ROS_ERROR int pointer * argv
Definition: roseus.cpp:819
E_NOSTRING
pointer EUSTF_TRANSFORM_LISTENER_DISPOSE(register context *ctx, int n, pointer *argv)
Definition: eustf.cpp:583
pointer EUSTF_SETTRANSFORM(register context *ctx, int n, pointer *argv)
Definition: eustf.cpp:135
pointer EUSTF_WAITFORTRANSFORM(register context *ctx, int n, pointer *argv)
Definition: eustf.cpp:162
pointer EUSTF_TRANSFORM_BROADCASTER(register context *ctx, int n, pointer *argv)
Definition: eustf.cpp:628
pointer EUSTF_CLEAR(register context *ctx, int n, pointer *argv)
Definition: eustf.cpp:348
pointer EUSTF_CANTRANSFORM(register context *ctx, int n, pointer *argv)
Definition: eustf.cpp:266
ROSCPP_DECL bool ok()
short s
void sendTransform(const StampedTransform &transform)
pointer EUSTF_GETPARENT(register context *ctx, int n, pointer *argv)
Definition: eustf.cpp:604
pointer EUSTF_GETFRAMESTRINGS(register context *ctx, int n, pointer *argv)
Definition: eustf.cpp:368
pointer error(enum errorcode ec,...) pointer error(va_alist) va_dcl
long eusinteger_t
#define ROS_WARN_STREAM(args)
#define ROS_DEBUG_STREAM(args)
TFSIMD_FORCE_INLINE Vector3 & getOrigin()
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
pointer defun(context *, char *, pointer, pointer(*)(), char *)
pointer PACKAGE
E_NONUMBER
pointer EUSTF_SETEXTRAPOLATIONLIMIT(register context *ctx, int n, pointer *argv)
Definition: eustf.cpp:592
pointer EUSTF_CANTRANSFORMFULL(register context *ctx, int n, pointer *argv)
Definition: eustf.cpp:299
float fltval()
long iv[1]
void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const
Quaternion getRotation() const
pointer makestring(char *, int)
pointer EUSTF_TFBC_WAITFORSERVER(register context *ctx, int n, pointer *argv)
Definition: eustf.cpp:712
TFSIMD_FORCE_INLINE const tfScalar & getX() const
pointer EUSTF_TFBC_LOOKUPTRANSFORM(register context *ctx, int n, pointer *argv)
Definition: eustf.cpp:776
pointer findpkg()
E_NOVECTOR
pointer EUSTF_SEND_TRANSFORM(register context *ctx, int n, pointer *argv)
Definition: eustf.cpp:634
#define ROS_ERROR_STREAM(args)
eusinteger_t intval(pointer p)
pointer EUSTF_GETLATERSTCOMMONTIME(register context *ctx, int n, pointer *argv)
Definition: eustf.cpp:383
void lookupTwist(const std::string &tracking_frame, const std::string &observation_frame, const std::string &reference_frame, const tf::Point &reference_point, const std::string &reference_point_frame, const ros::Time &time, const ros::Duration &averaging_interval, geometry_msgs::Twist &twist) const
double eusfloat_t
pointer EUSTF_LOOKUPTRANSFORMFULL(register context *ctx, int n, pointer *argv)
Definition: eustf.cpp:442
pointer EUSTF_BUFFER_CLIENT(register context *ctx, int n, pointer *argv)
Definition: eustf.cpp:676
pointer NIL
virtual geometry_msgs::TransformStamped lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout=ros::Duration(0.0)) const
pointer EUSTF_FRAMEEXISTS(register context *ctx, int n, pointer *argv)
Definition: eustf.cpp:356
#define ROS_ERROR(...)
TFSIMD_FORCE_INLINE void setOrigin(const Vector3 &origin)
void setExtrapolationLimit(const ros::Duration &distance)
bool frameExists(const std::string &frame_id_str) const
virtual bool canTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, const ros::Duration timeout=ros::Duration(0.0), std::string *errstr=NULL) const
std::string frame_id_
pointer length
pointer EUSTF_TFBC_CANTRANSFORM(register context *ctx, int n, pointer *argv)
Definition: eustf.cpp:727
#define set_ros_time(time, argv)
Definition: eustf.cpp:109
struct floatvector fvec


roseus
Author(s): Kei Okada
autogenerated on Thu Apr 4 2019 02:55:35