eustf.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2008, JSK Lab, the Univ. of Tokyo
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00035 // author: Ryohei Ueda
00036 #include <stdio.h>
00037 #include <stdlib.h>
00038 #include <unistd.h>
00039 #include <string.h>
00040 #include <signal.h>
00041 #include <math.h>
00042 #include <time.h>
00043 #include <pthread.h>
00044 #include <setjmp.h>
00045 #include <errno.h>
00046 
00047 #include <list>
00048 #include <vector>
00049 #include <set>
00050 #include <string>
00051 #include <map>
00052 #include <sstream>
00053 
00054 #include <cstdio>
00055 #include <boost/thread/mutex.hpp>
00056 #include <boost/thread/condition.hpp>
00057 #include <boost/shared_ptr.hpp>
00058 
00059 #include <ros/init.h>
00060 #include <ros/time.h>
00061 #include <ros/rate.h>
00062 #include <ros/master.h>
00063 #include <ros/this_node.h>
00064 #include <ros/node_handle.h>
00065 #include <ros/service.h>
00066 #include <tf/tf.h>
00067 #include <tf/transform_listener.h>
00068 #include <tf/transform_datatypes.h>
00069 #include <tf/transform_broadcaster.h>
00070 
00071 #include <tf2_ros/buffer_client.h>
00072 #ifdef TF2_ROS_VERSION_3 // if this is groovy
00073 #define tf2_ros tf2
00074 #endif
00075 
00076 // for eus.h
00077 #define class   eus_class
00078 #define throw   eus_throw
00079 #define export  eus_export
00080 #define vector  eus_vector
00081 #define string  eus_string
00082 #ifdef __clang__
00083 #undef MAX
00084 #undef MIN
00085 #endif
00086 #include "eus.h"
00087 
00088 extern "C" {
00089   pointer ___eustf(register context *ctx, int n, pointer *argv, pointer env);
00090   void register_eustf(){
00091     char modname[] = "___eustf";
00092     return add_module_initializer(modname, (pointer (*)())___eustf);}
00093 }
00094 
00095 #undef class
00096 #undef throw
00097 #undef export
00098 #undef vector
00099 #undef string
00100 
00101 using namespace ros;
00102 using namespace std;
00103 
00104 
00105 /***********************************************************
00106  *   TF wrapper
00107  ************************************************************/
00108 
00109 #define set_ros_time(time,argv)                         \
00110   if (isvector(argv) && (elmtypeof(argv)==ELM_INT)) {   \
00111     time.sec  = argv->c.ivec.iv[0];                     \
00112     time.nsec = argv->c.ivec.iv[1];                     \
00113   } else {                                              \
00114     error(E_NOVECTOR);                                  \
00115   }
00116 
00117 /* */
00118 pointer EUSTF_TRANSFORMER(register context *ctx,int n,pointer *argv)
00119 {
00120   numunion nu;
00121   ckarg(2);
00122   bool interpolating = ((argv[0]==T)?true:false);
00123   float cache_time = ckfltval(argv[1]);
00124   return(makeint((eusinteger_t)(new tf::Transformer(interpolating, ros::Duration(cache_time)))));
00125 }
00126 
00127 pointer EUSTF_ALLFRAMESASSTRING(register context *ctx,int n,pointer *argv)
00128 {
00129   ckarg(1);
00130   tf::Transformer *tf = (tf::Transformer *)(intval(argv[0]));
00131   string str = tf->allFramesAsString();
00132   return(makestring((char *)(str.c_str()), str.length()));
00133 }
00134 
00135 pointer EUSTF_SETTRANSFORM(register context *ctx,int n,pointer *argv)
00136 {
00137   ckarg(7);
00138   tf::Transformer *tf = (tf::Transformer *)(intval(argv[0]));
00139   if (!isvector(argv[1])) error(E_NOVECTOR);
00140   if (!isvector(argv[2])) error(E_NOVECTOR);
00141   eusfloat_t *pos = argv[1]->c.fvec.fv;
00142   eusfloat_t *rot = argv[2]->c.fvec.fv;
00143   isintvector(argv[3]);
00144   eusinteger_t *stamp = argv[3]->c.ivec.iv;
00145   if (!isstring(argv[4])) error(E_NOSTRING);
00146   if (!isstring(argv[5])) error(E_NOSTRING);
00147   if (!isstring(argv[6])) error(E_NOSTRING);
00148   std::string frame_id = std::string((char*)(argv[4]->c.str.chars));
00149   std::string child_frame_id = std::string((char*)(argv[5]->c.str.chars));
00150   std::string authority= std::string((char*)(argv[6]->c.str.chars));
00151   tf::StampedTransform transform;
00152   transform.setOrigin(tf::Vector3(pos[0], pos[1], pos[2]));
00153   transform.setRotation(tf::Quaternion(rot[3], rot[0], rot[1], rot[2]));
00154   transform.frame_id_ = frame_id;
00155   transform.child_frame_id_ = child_frame_id;
00156   transform.stamp_.sec = stamp[0];
00157   transform.stamp_.nsec = stamp[1];
00158   bool ret = tf->setTransform(transform, authority);
00159   return(ret?T:NIL);
00160 }
00161 
00162 pointer EUSTF_WAITFORTRANSFORM(register context *ctx,int n,pointer *argv)
00163 {
00164   numunion nu;
00165   ckarg(6);
00166   tf::Transformer *tf;
00167   std::string target_frame, source_frame;
00168   ros::Time time;
00169   float timeout = 0, duration = 0;
00170   bool ret;
00171 
00172   tf = (tf::Transformer *)(intval(argv[0]));
00173   if (isstring(argv[1]))
00174     target_frame = std::string((char*)(argv[1]->c.str.chars));
00175   else error(E_NOSTRING);
00176 
00177   if (isstring(argv[2]))
00178     source_frame = std::string((char*)(argv[2]->c.str.chars));
00179   else error(E_NOSTRING);
00180 
00181   set_ros_time(time,argv[3]);
00182 
00183   if (isint(argv[4])) timeout = (float)intval(argv[4]);
00184   else if (isflt(argv[4])) timeout = (float)fltval(argv[4]);
00185   else error(E_NONUMBER);
00186 
00187   if (isint(argv[5])) duration = (float)intval(argv[5]);
00188   else if (isflt(argv[5])) duration = (float)fltval(argv[5]);
00189   else error(E_NONUMBER);
00190 
00191   std::string err_str = std::string();
00192   ret = tf->waitForTransform(target_frame, source_frame, time,
00193                              ros::Duration(timeout), ros::Duration(duration),
00194                              &err_str);
00195   if(!ret) {
00196     ROS_WARN_STREAM("waitForTransform failed! : " << err_str);
00197   }
00198   ROS_DEBUG_STREAM("waitForTransform : "
00199                    << "target_frame : " << target_frame
00200                    << "source_frame : " << source_frame
00201                    << "time : " << time
00202                    << "timeout : " << timeout
00203                    << "duration : " << duration
00204                    << "return : " << ret);
00205 
00206   return((ret==true)?(T):(NIL));
00207 }
00208 
00209 pointer EUSTF_WAITFORTRANSFORMFULL(register context *ctx,int n,pointer *argv)
00210 {
00211   numunion nu;
00212   ckarg(8);
00213   tf::Transformer *tf;
00214   std::string target_frame, source_frame, fixed_frame;
00215   ros::Time target_time, source_time;
00216   float timeout = 0, duration = 0;
00217   bool ret;
00218 
00219   tf = (tf::Transformer *)(intval(argv[0]));
00220   if (isstring(argv[1]))
00221     target_frame = std::string((char*)(argv[1]->c.str.chars));
00222   else error(E_NOSTRING);
00223 
00224   set_ros_time(target_time,argv[2]);
00225 
00226   if (isstring(argv[3]))
00227     source_frame = std::string((char*)(argv[3]->c.str.chars));
00228   else error(E_NOSTRING);
00229 
00230   set_ros_time(source_time,argv[4]);
00231 
00232   if (isstring(argv[5]))
00233     fixed_frame = std::string((char*)(argv[5]->c.str.chars));
00234   else error(E_NOSTRING);
00235 
00236   if (isint(argv[6])) timeout = (float)intval(argv[6]);
00237   else if (isflt(argv[6])) timeout = (float)fltval(argv[6]);
00238   else error(E_NONUMBER);
00239 
00240   if (isint(argv[7])) duration = (float)intval(argv[7]);
00241   else if (isflt(argv[7])) duration = (float)fltval(argv[7]);
00242   else error(E_NONUMBER);
00243 
00244   std::string err_str = std::string();
00245   ret = tf->waitForTransform(target_frame, target_time,
00246                              source_frame, source_time,
00247                              fixed_frame,
00248                              ros::Duration(timeout), ros::Duration(duration),
00249                              &err_str);
00250   if(!ret) {
00251     ROS_WARN_STREAM("waitForTransformFull failed! : " << err_str);
00252   }
00253   ROS_DEBUG_STREAM("waitForTransformFull : "
00254                    << "target_frame : " << target_frame
00255                    << "target_time : " << target_time
00256                    << "source_frame : " << source_frame
00257                    << "source_time : " << source_time
00258                    << "fixed_frame : " << fixed_frame
00259                    << "timeout : " << timeout
00260                    << "duration : " << duration
00261                    << "return : " << ret);
00262 
00263   return((ret==true)?(T):(NIL));
00264 }
00265 
00266 pointer EUSTF_CANTRANSFORM(register context *ctx,int n,pointer *argv)
00267 {
00268   ckarg(4);
00269   tf::Transformer *tf;
00270   std::string target_frame, source_frame;
00271   ros::Time time;
00272   bool ret;
00273 
00274   tf = (tf::Transformer *)(intval(argv[0]));
00275   if (isstring(argv[1]))
00276     target_frame = std::string((char*)(argv[1]->c.str.chars)); 
00277   else error(E_NOSTRING);
00278 
00279   if (isstring(argv[2]))
00280     source_frame = std::string((char*)(argv[2]->c.str.chars));
00281   else error(E_NOSTRING);
00282 
00283   set_ros_time(time,argv[3]);
00284 
00285   std::string err_str = std::string();
00286   ret = tf->canTransform(target_frame, source_frame, time, &err_str);
00287   if(!ret) {
00288     ROS_WARN_STREAM("canTransform " << target_frame << " " << source_frame << " failed! : " << err_str);
00289   }
00290   ROS_DEBUG_STREAM("canTransform : "
00291                    << "target_frame : " << target_frame
00292                    << "source_frame : " << source_frame
00293                    << "time : " << time
00294                    << "return : " << ret);
00295 
00296   return((ret==true)?(T):(NIL));
00297 }
00298 
00299 pointer EUSTF_CANTRANSFORMFULL(register context *ctx,int n,pointer *argv)
00300 {
00301   ckarg(7);
00302   tf::Transformer *tf;
00303   std::string target_frame, source_frame, fixed_frame;
00304   ros::Time target_time, source_time;
00305   bool ret;
00306 
00307   tf = (tf::Transformer *)(intval(argv[0]));
00308   if (isstring(argv[1]))
00309     target_frame = std::string((char*)(argv[1]->c.str.chars));
00310   else error(E_NOSTRING);
00311 
00312   set_ros_time(target_time,argv[3]);
00313 
00314   if (isstring(argv[3]))
00315     source_frame = std::string((char*)(argv[3]->c.str.chars));
00316   else error(E_NOSTRING);
00317 
00318   set_ros_time(source_time,argv[4]);
00319 
00320   if (isstring(argv[5]))
00321     fixed_frame = std::string((char*)(argv[5]->c.str.chars));
00322   else error(E_NOSTRING);
00323 
00324   std::string err_str = std::string();
00325   ret = tf->canTransform(target_frame, target_time,
00326                          source_frame, source_time,
00327                          fixed_frame, &err_str);
00328   if(!ret) {
00329     ROS_WARN_STREAM("canTransformFull " << target_frame << " " << source_frame << " failed! : " << err_str);
00330   }
00331   ROS_DEBUG_STREAM("canTransformFull : "
00332                    << "target_frame : " << target_frame
00333                    << "target_time : " << target_time
00334                    << "source_frame : " << source_frame
00335                    << "source_time : " << source_time
00336                    << "fixed_frame : " << fixed_frame
00337                    << "return : " << ret);
00338 
00339   return((ret==true)?(T):(NIL));
00340 }
00341 
00342 pointer EUSTF_CHAIN(register context *ctx,int n,pointer *argv)
00343 {
00344   ROS_ERROR("%s is not implemented yet", __PRETTY_FUNCTION__);
00345   return(T);
00346 }
00347 
00348 pointer EUSTF_CLEAR(register context *ctx,int n,pointer *argv)
00349 {
00350   ckarg(1);
00351   tf::Transformer *tf = (tf::Transformer *)(intval(argv[0]));
00352   tf->clear();
00353   return(T);
00354 }
00355 
00356 pointer EUSTF_FRAMEEXISTS(register context *ctx,int n,pointer *argv)
00357 {
00358   ckarg(2);
00359   tf::Transformer *tf;
00360   std::string frame_id;
00361 
00362   tf = (tf::Transformer *)(intval(argv[0]));
00363   if (!isstring(argv[1])) error(E_NOSTRING);
00364   frame_id = std::string((char*)(argv[1]->c.str.chars));
00365   return(tf->frameExists(frame_id)?T:NIL);
00366 }
00367 
00368 pointer EUSTF_GETFRAMESTRINGS(register context *ctx,int n,pointer *argv)
00369 {
00370   ckarg(1);
00371   tf::Transformer *tf = (tf::Transformer *)(intval(argv[0]));
00372   std::vector< std::string > ids;
00373   pointer str = NIL;
00374 
00375   tf->getFrameStrings(ids);
00376   for (std::vector< std::string >::iterator s = ids.begin(); s != ids.end(); s++) {
00377     str=cons(ctx,makestring((char *)(s->c_str()),s->length()),str);
00378   }
00379 
00380   return(str);
00381 }
00382 
00383 pointer EUSTF_GETLATERSTCOMMONTIME(register context *ctx,int n,pointer *argv)
00384 {
00385   ckarg(3);
00386   tf::Transformer *tf;
00387   std::string source_frame, target_frame, error_string;
00388 
00389   tf = (tf::Transformer *)(intval(argv[0]));
00390   if (!isstring(argv[1])) error(E_NOSTRING);
00391   source_frame = std::string((char*)(argv[1]->c.str.chars));
00392   if (!isstring(argv[2])) error(E_NOSTRING);
00393   target_frame = std::string((char*)(argv[2]->c.str.chars));
00394 
00395   ros::Time time;
00396   int r = tf->getLatestCommonTime(source_frame, target_frame, time, &error_string);
00397   if ( r == 0 ) {
00398     return(cons(ctx,makeint(time.sec),makeint(time.nsec)));
00399   } else {
00400     ROS_ERROR_STREAM("getLatestCommonTime " << target_frame << " " << source_frame << " failed! : " << error_string);
00401     return(NIL);
00402   }
00403 }
00404 
00405 pointer EUSTF_LOOKUPTRANSFORM(register context *ctx,int n,pointer *argv)
00406 {
00407   ckarg(4);
00408   tf::Transformer *tf;
00409   std::string target_frame, source_frame;
00410   ros::Time time;
00411 
00412   tf = (tf::Transformer *)(intval(argv[0]));
00413   if (!isstring(argv[1])) error(E_NOSTRING);
00414   target_frame = std::string((char*)(argv[1]->c.str.chars));
00415   if (!isstring(argv[2])) error(E_NOSTRING);
00416   source_frame = std::string((char*)(argv[2]->c.str.chars));
00417 
00418   set_ros_time(time,argv[3]);
00419 
00420   tf::StampedTransform transform;
00421   try {
00422     tf->lookupTransform(target_frame, source_frame, time, transform);
00423   } catch ( std::runtime_error e ) {
00424     ROS_ERROR("%s",e.what()); return(NIL);
00425   }
00426 
00427   pointer vs = makefvector(7);          //pos[3] + rot[4](angle-axis quaternion)
00428   vpush(vs);
00429   tf::Vector3 p = transform.getOrigin();
00430   tf::Quaternion q = transform.getRotation();
00431   vs->c.fvec.fv[0] = p.getX();
00432   vs->c.fvec.fv[1] = p.getY();
00433   vs->c.fvec.fv[2] = p.getZ();
00434   vs->c.fvec.fv[3] = q.getW();
00435   vs->c.fvec.fv[4] = q.getX();
00436   vs->c.fvec.fv[5] = q.getY();
00437   vs->c.fvec.fv[6] = q.getZ();
00438   vpop();
00439   return(vs);
00440 }
00441 
00442 pointer EUSTF_LOOKUPTRANSFORMFULL(register context *ctx,int n,pointer *argv)
00443 {
00444   ckarg(6);
00445   tf::Transformer *tf;
00446   std::string target_frame, source_frame, fixed_frame;
00447   ros::Time target_time, source_time;
00448 
00449   tf = (tf::Transformer *)(intval(argv[0]));
00450   if (!isstring(argv[1])) error(E_NOSTRING);
00451   target_frame = std::string((char*)(argv[1]->c.str.chars));
00452 
00453   set_ros_time(target_time,argv[2]);
00454 
00455   if (!isstring(argv[3])) error(E_NOSTRING);
00456   source_frame = std::string((char*)(argv[3]->c.str.chars));
00457 
00458   set_ros_time(source_time,argv[4]);
00459 
00460   if (!isstring(argv[5])) error(E_NOSTRING);
00461   fixed_frame = std::string((char*)(argv[5]->c.str.chars));
00462 
00463   tf::StampedTransform transform;
00464   try {
00465     tf->lookupTransform(target_frame, target_time,
00466                         source_frame, source_time, fixed_frame, transform);
00467   } catch ( std::runtime_error e ) {
00468     ROS_ERROR("%s",e.what()); return(NIL);
00469   }
00470 
00471   pointer vs = makefvector(7);          //pos[3] + rot[4](angle-axis quaternion)
00472   vpush(vs);
00473   tf::Vector3 p = transform.getOrigin();
00474   tf::Quaternion q = transform.getRotation();
00475   vs->c.fvec.fv[0] = p.getX();
00476   vs->c.fvec.fv[1] = p.getY();
00477   vs->c.fvec.fv[2] = p.getZ();
00478   vs->c.fvec.fv[3] = q.getW();
00479   vs->c.fvec.fv[4] = q.getX();
00480   vs->c.fvec.fv[5] = q.getY();
00481   vs->c.fvec.fv[6] = q.getZ();
00482   vpop();
00483   return(vs);
00484 }
00485 
00486 
00487 pointer EUSTF_TRANSFORMPOSE(register context *ctx,int n,pointer *argv)
00488 {
00489   ckarg(5);                     // tf, target_frame, time, frame_id, 
00490                                 // pose as float-vector. its vector is a vector
00491                                 // appended position and angle-vector quaternion
00492   tf::TransformListener *tf = (tf::TransformListener *)(intval(argv[0]));
00493   if (!isstring(argv[1])) error(E_NOSTRING);
00494   std::string target_frame = std::string((char*)(argv[1]->c.str.chars));
00495   ros::Time tm;
00496   set_ros_time(tm, argv[2]);
00497   
00498   if (!isstring(argv[3])) error(E_NOSTRING);
00499   std::string frame_id = std::string((char*)(argv[3]->c.str.chars));
00500   
00501   geometry_msgs::PoseStamped input, output;
00502   // setup input
00503   input.pose.position.x = argv[4]->c.fvec.fv[0];
00504   input.pose.position.y = argv[4]->c.fvec.fv[1];
00505   input.pose.position.z = argv[4]->c.fvec.fv[2];
00506   input.pose.orientation.w = argv[4]->c.fvec.fv[3]; // angle-vector format
00507   input.pose.orientation.x = argv[4]->c.fvec.fv[4];
00508   input.pose.orientation.y = argv[4]->c.fvec.fv[5];
00509   input.pose.orientation.z = argv[4]->c.fvec.fv[6];
00510   //  input.header.
00511   input.header.stamp = tm;
00512   input.header.frame_id = frame_id;
00513 
00514   try {
00515     tf->transformPose(target_frame, input, output);
00516   } catch ( std::runtime_error e ) {
00517     ROS_ERROR("%s",e.what()); return(NIL);
00518   }
00519 
00520   pointer vs = makefvector(7); //pos[3] + rot[4](angle-axis quaternion)
00521   vpush(vs);
00522   vs->c.fvec.fv[0] = output.pose.position.x;
00523   vs->c.fvec.fv[1] = output.pose.position.y;
00524   vs->c.fvec.fv[2] = output.pose.position.z;
00525   vs->c.fvec.fv[3] = output.pose.orientation.w;
00526   vs->c.fvec.fv[4] = output.pose.orientation.x;
00527   vs->c.fvec.fv[5] = output.pose.orientation.y;
00528   vs->c.fvec.fv[6] = output.pose.orientation.z;
00529   vpop();
00530   return(vs);
00531 }
00532 
00533 pointer EUSTF_LOOKUPVELOCITY(register context *ctx,int n,pointer *argv)
00534 {
00535   numunion nu;
00536   ckarg(4);
00537   tf::Transformer *tf;
00538   std::string reference_frame, moving_frame;
00539   float time = 0, duration = 0;
00540 
00541   tf = (tf::Transformer *)(intval(argv[0]));
00542   if (!isstring(argv[1])) error(E_NOSTRING);
00543   reference_frame = std::string((char*)(argv[1]->c.str.chars));
00544 
00545   if (!isstring(argv[2])) error(E_NOSTRING);
00546   moving_frame = std::string((char*)(argv[2]->c.str.chars));
00547 
00548   if (isint(argv[3])) time = (float)intval(argv[3]);
00549   else if (isflt(argv[3])) time = (float)fltval(argv[3]);
00550   else error(E_NONUMBER);
00551 
00552   if (isint(argv[4])) duration = (float)intval(argv[4]);
00553   else if (isflt(argv[4])) duration = (float)fltval(argv[4]);
00554   else error(E_NONUMBER);
00555 
00556   geometry_msgs::Twist velocity;
00557   // ROS_ERROR("%s is not implemented yet since lookupVelocity seems obsoluted", __PRETTY_FUNCTION__);
00558   tf->lookupTwist(reference_frame, moving_frame, ros::Time(time), ros::Duration(duration), velocity);
00559 
00560   pointer vs = makefvector(6);          //pos[3] + rot[3](angle-axis)
00561   vpush(vs);
00562   vs->c.fvec.fv[0] = velocity.linear.x;
00563   vs->c.fvec.fv[1] = velocity.linear.y;
00564   vs->c.fvec.fv[2] = velocity.linear.z;
00565   vs->c.fvec.fv[3] = velocity.angular.x;
00566   vs->c.fvec.fv[4] = velocity.angular.y;
00567   vs->c.fvec.fv[5] = velocity.angular.z;
00568   vpop();
00569   return(vs);
00570 }
00571 
00572 /* */
00573 pointer EUSTF_TRANSFORM_LISTENER(register context *ctx,int n,pointer *argv)
00574 {
00575   if( ! ros::ok() ) { error(E_USER,"You must call ros::init() before creating the first NodeHandle"); }
00576   numunion nu;
00577   ckarg(2);
00578   float cache_time = ckfltval(argv[0]);
00579   bool spin_thread = ((argv[1]==T)?true:false);
00580   return(makeint((eusinteger_t)(new tf::TransformListener(ros::Duration(cache_time), spin_thread))));
00581 }
00582 
00583 pointer EUSTF_TRANSFORM_LISTENER_DISPOSE(register context *ctx,int n,pointer *argv)
00584 {
00585   ckarg(1);
00586   tf::TransformListener *tf = (tf::TransformListener *)(intval(argv[0]));
00587   delete(tf);
00588   return(T);
00589 }
00590 
00591 /* */
00592 pointer EUSTF_SETEXTRAPOLATIONLIMIT(register context *ctx,int n,pointer *argv)
00593 {
00594   numunion nu;
00595   ckarg(2);
00596   tf::Transformer *tf;
00597   tf = (tf::Transformer *)(intval(argv[0]));
00598   float distance = ckfltval(argv[1]);
00599 
00600   tf->setExtrapolationLimit(ros::Duration(distance));
00601   return(T);
00602 }
00603 
00604 pointer EUSTF_GETPARENT(register context *ctx,int n,pointer *argv)
00605 {
00606   ckarg(3);
00607   tf::Transformer *tf;
00608   std::string frame_id, parent;
00609   ros::Time time;
00610 
00611   tf = (tf::Transformer *)(intval(argv[0]));
00612 
00613   if (isstring(argv[1]))
00614     frame_id = std::string((char*)(argv[1]->c.str.chars));
00615   else error(E_NOSTRING);
00616 
00617   set_ros_time(time,argv[2]);
00618 
00619   try {
00620     bool ret = tf->getParent(frame_id, time, parent);
00621     return(ret?makestring((char *)parent.c_str(),parent.length()):NIL);
00622   } catch ( std::runtime_error e ) {
00623     ROS_ERROR("%s",e.what()); return(NIL);
00624   }
00625 }
00626 
00627 /* */
00628 pointer EUSTF_TRANSFORM_BROADCASTER(register context *ctx,int n,pointer *argv)
00629 {
00630   if( ! ros::ok() ) { error(E_USER,"You must call ros::init() before creating the first NodeHandle"); }
00631   return(makeint((eusinteger_t)(new tf::TransformBroadcaster())));
00632 }
00633 
00634 pointer EUSTF_SEND_TRANSFORM(register context *ctx,int n,pointer *argv){
00635 
00636   /* ptr pos quarternion parent_frame_id, child_frame_id, time */
00637   ckarg(6);
00638 
00639   tf::TransformBroadcaster *bc = (tf::TransformBroadcaster *)(intval(argv[0]));
00640 
00641   isintvector(argv[5]);
00642   ros::Time tm;
00643   tm.sec = argv[5]->c.ivec.iv[0];
00644   tm.nsec = argv[5]->c.ivec.iv[1];
00645 
00646   eusfloat_t *pos, *quaternion;
00647   std::string p_frame_id, c_frame_id;
00648   isfltvector(argv[1]);
00649   isfltvector(argv[2]);
00650   isstring(argv[3]);
00651   isstring(argv[4]);
00652   pos = argv[1]->c.fvec.fv;
00653   quaternion= argv[2]->c.fvec.fv;
00654   p_frame_id = (char *)argv[3]->c.str.chars;
00655   c_frame_id = (char *)argv[4]->c.str.chars;
00656 
00657   geometry_msgs::TransformStamped trans;
00658   trans.header.stamp = tm;
00659   trans.header.frame_id = p_frame_id;
00660   trans.child_frame_id = c_frame_id;
00661   trans.transform.translation.x = pos[0]/1000.0;
00662   trans.transform.translation.y = pos[1]/1000.0;
00663   trans.transform.translation.z = pos[2]/1000.0;
00664 
00665   trans.transform.rotation.w = quaternion[0];
00666   trans.transform.rotation.x = quaternion[1];
00667   trans.transform.rotation.y = quaternion[2];
00668   trans.transform.rotation.z = quaternion[3];
00669 
00670   bc->sendTransform(trans);
00671 
00672   return (T);
00673 }
00674 
00675 /* tf2 */
00676 pointer EUSTF_BUFFER_CLIENT(register context *ctx,int n,pointer *argv)
00677 {
00678   if(!ros::ok()) { error(E_USER, "You must call (ros::roseus \"nodename\") before creating the first NodeHandle"); }
00679   /* &optional (ns "tf2_buffer_server") (check_frequency 10.0) (timeout_padding 2.0) */
00680   numunion nu;
00681   std::string ns_str ("tf2_buffer_server");
00682   double check_frequency = 10.0;
00683   ros::Duration timeout_padding(2.0);
00684 
00685   ckarg2(0, 3);
00686   if (n > 0) {
00687     if (isstring (argv[0])) {
00688       ns_str.assign ((char *)(argv[0]->c.str.chars));
00689     } else {
00690       error(E_NOSTRING);
00691     }
00692   }
00693   if (n > 1) {
00694     check_frequency = ckfltval(argv[1]);
00695   }
00696   if (n > 2) {
00697     eusfloat_t pd = ckfltval(argv[2]);
00698     timeout_padding = ros::Duration(pd);
00699   }
00700 
00701   return(makeint((eusinteger_t)(new tf2_ros::BufferClient (ns_str, check_frequency, timeout_padding))));
00702 }
00703 
00704 pointer EUSTF_BUFFER_CLIENT_DISPOSE(register context *ctx,int n,pointer *argv)
00705 {
00706   ckarg(1);
00707   tf2_ros::BufferClient *tfbc = (tf2_ros::BufferClient *)(intval(argv[0]));
00708   delete(tfbc);
00709   return(T);
00710 }
00711 
00712 pointer EUSTF_TFBC_WAITFORSERVER(register context *ctx,int n,pointer *argv)
00713 {
00714   ckarg2(1, 2);
00715   tf2_ros::BufferClient *tfbc = (tf2_ros::BufferClient *)(intval(argv[0]));
00716   numunion nu;
00717   bool ret;
00718   if (n > 1) {
00719     ros::Duration tm(ckfltval(argv[1]));
00720     ret = tfbc->waitForServer(tm);
00721   } else {
00722     ret = tfbc->waitForServer();
00723   }
00724   return((ret==true)?(T):(NIL));
00725 }
00726 
00727 pointer EUSTF_TFBC_CANTRANSFORM(register context *ctx,int n,pointer *argv)
00728 {
00729   ckarg2(4, 5);
00730   tf2_ros::BufferClient *tfbc = (tf2_ros::BufferClient *)(intval(argv[0]));
00731   numunion nu;
00732   std::string target_frame, source_frame;
00733   ros::Time time;
00734   ros::Duration timeout(0.0);
00735   bool ret;
00736 
00737   if (isstring(argv[1])) {
00738     char *cstr = (char*)(argv[1]->c.str.chars);
00739     if (cstr[0] == '/') {
00740       target_frame.assign((char *)(cstr+1));
00741     } else {
00742       target_frame.assign(cstr);
00743     }
00744   } else error(E_NOSTRING);
00745 
00746   if (isstring(argv[2])) {
00747     char *cstr = (char*)(argv[2]->c.str.chars);
00748     if (cstr[0] == '/') {
00749       source_frame.assign((char *)(cstr+1));
00750     } else {
00751       source_frame.assign(cstr);
00752     }
00753   } else error(E_NOSTRING);
00754 
00755   set_ros_time(time, argv[3]);
00756 
00757   if (n > 4) {
00758     timeout = ros::Duration(ckfltval(argv[4]));
00759   }
00760   //target_frame.
00761   std::string err_str = std::string();
00762   ret = tfbc->canTransform(target_frame, source_frame, time, timeout, &err_str);
00763   if(!ret) {
00764     ROS_WARN_STREAM("BufferClient::waitForTransform failed! : " << err_str);
00765   }
00766   ROS_DEBUG_STREAM("BufferClient::waitForTransform : "
00767                    << "target_frame : " << target_frame
00768                    << "source_frame : " << source_frame
00769                    << "time : " << time
00770                    << "timeout : " << timeout
00771                    << "return : " << ret);
00772 
00773   return((ret==true)?(T):(NIL));
00774 }
00775 
00776 pointer EUSTF_TFBC_LOOKUPTRANSFORM(register context *ctx,int n,pointer *argv)
00777 {
00778   ckarg2(4, 5);
00779   tf2_ros::BufferClient *tfbc = (tf2_ros::BufferClient *)(intval(argv[0]));
00780   numunion nu;
00781   std::string target_frame, source_frame;
00782   ros::Time time;
00783   ros::Duration timeout(0.0);
00784 
00785   if (!isstring(argv[1])) error(E_NOSTRING);
00786   target_frame = std::string((char*)(argv[1]->c.str.chars));
00787 
00788   if (!isstring(argv[2])) error(E_NOSTRING);
00789   source_frame = std::string((char*)(argv[2]->c.str.chars));
00790 
00791   set_ros_time(time, argv[3]);
00792 
00793   if (n > 4) {
00794     timeout = ros::Duration(ckfltval(argv[4]));
00795   }
00796 
00797   geometry_msgs::TransformStamped trans;
00798   try {
00799     trans = tfbc->lookupTransform(target_frame, source_frame, time, timeout);
00800   } catch (std::runtime_error e) {
00801     ROS_ERROR("%s", e.what()); return(NIL);
00802   }
00803 
00804   pointer vs = makefvector(7);  //pos[3] + rot[4](angle-axis quaternion)
00805   vpush(vs);
00806   //trans.header.frame_id
00807   //trans.child_frame_id
00808   //trans.header.stamp --> ??
00809   vs->c.fvec.fv[0] = trans.transform.translation.x;
00810   vs->c.fvec.fv[1] = trans.transform.translation.y;
00811   vs->c.fvec.fv[2] = trans.transform.translation.z;
00812   vs->c.fvec.fv[3] = trans.transform.rotation.w;
00813   vs->c.fvec.fv[4] = trans.transform.rotation.x;
00814   vs->c.fvec.fv[5] = trans.transform.rotation.y;
00815   vs->c.fvec.fv[6] = trans.transform.rotation.z;
00816   vpop();
00817 
00818   return(vs);
00819 }
00820 
00821 pointer ___eustf(register context *ctx, int n, pointer *argv, pointer env)
00822 {
00823   pointer rospkg,p=Spevalof(PACKAGE);
00824   rospkg=findpkg(makestring("TF",2));
00825   if (rospkg == 0) rospkg=makepkg(ctx,makestring("TF", 2),NIL,NIL);
00826   Spevalof(PACKAGE)=rospkg;
00827 
00828   rospkg=findpkg(makestring("ROS",3));
00829   if (rospkg == 0 ) {
00830     ROS_ERROR("Coudld not found ROS package; Please load eusros.so");
00831     exit(2);
00832   }
00833   Spevalof(PACKAGE)=rospkg;
00834   defun(ctx,"EUSTF-TRANSFORMER",argv[0],(pointer (*)())EUSTF_TRANSFORMER);
00835   defun(ctx,"EUSTF-ALL-FRAMES-AS-STRING",argv[0],(pointer (*)())EUSTF_ALLFRAMESASSTRING);
00836   defun(ctx,"EUSTF-SET-TRANSFORM",argv[0],(pointer (*)())EUSTF_SETTRANSFORM);
00837   defun(ctx,"EUSTF-WAIT-FOR-TRANSFORM",argv[0],(pointer (*)())EUSTF_WAITFORTRANSFORM);
00838   defun(ctx,"EUSTF-WAIT-FOR-TRANSFORM-FULL",argv[0],(pointer (*)())EUSTF_WAITFORTRANSFORMFULL);
00839   defun(ctx,"EUSTF-CAN-TRANSFORM",argv[0],(pointer (*)())EUSTF_CANTRANSFORM);
00840   defun(ctx,"EUSTF-CAN-TRANSFORM-FULL",argv[0],(pointer (*)())EUSTF_CANTRANSFORMFULL);
00841   defun(ctx,"EUSTF-CHAIN",argv[0],(pointer (*)())EUSTF_CHAIN);
00842   defun(ctx,"EUSTF-CLEAR",argv[0],(pointer (*)())EUSTF_CLEAR);
00843   defun(ctx,"EUSTF-FRAME-EXISTS",argv[0],(pointer (*)())EUSTF_FRAMEEXISTS);
00844   defun(ctx,"EUSTF-GET-FRAME-STRINGS",argv[0],(pointer (*)())EUSTF_GETFRAMESTRINGS);
00845   defun(ctx,"EUSTF-GET-LATEST-COMMON-TIME",argv[0],(pointer (*)())EUSTF_GETLATERSTCOMMONTIME);
00846   defun(ctx,"EUSTF-LOOKUP-TRANSFORM",argv[0],(pointer (*)())EUSTF_LOOKUPTRANSFORM);
00847   defun(ctx,"EUSTF-LOOKUP-TRANSFORM-FULL",argv[0],(pointer (*)())EUSTF_LOOKUPTRANSFORMFULL);
00848   defun(ctx,"EUSTF-TRANSFORM-POSE",argv[0],(pointer (*)())EUSTF_TRANSFORMPOSE);
00849   defun(ctx,"EUSTF-LOOKUP-VELOCITY",argv[0],(pointer (*)())EUSTF_LOOKUPVELOCITY);
00850   /* */
00851   defun(ctx,"EUSTF-TRANSFORM-LISTENER",argv[0],(pointer (*)())EUSTF_TRANSFORM_LISTENER);
00852   defun(ctx,"EUSTF-TRANSFORM-LISTENER-DISPOSE",argv[0],(pointer (*)())EUSTF_TRANSFORM_LISTENER_DISPOSE);
00853 
00854   /* */
00855   defun(ctx,"EUSTF-SET-EXTRAPOLATION-LIMIT",argv[0],(pointer (*)())EUSTF_SETEXTRAPOLATIONLIMIT);
00856   defun(ctx,"EUSTF-GET-PARENT",argv[0],(pointer (*)())EUSTF_GETPARENT);
00857   /* */
00858   defun(ctx,"EUSTF-TRANSFORM-BROADCASTER",argv[0],(pointer (*)())EUSTF_TRANSFORM_BROADCASTER);
00859   defun(ctx,"EUSTF-SEND-TRANSFORM",argv[0],(pointer (*)())EUSTF_SEND_TRANSFORM);
00860 
00861   /* tf2 */
00862   defun(ctx,"EUSTF-BUFFER-CLIENT",argv[0],(pointer (*)())EUSTF_BUFFER_CLIENT);
00863   defun(ctx,"EUSTF-BUFFER-CLIENT-DISPOSE",argv[0],(pointer (*)())EUSTF_BUFFER_CLIENT_DISPOSE);
00864   defun(ctx,"EUSTF-TF2-WAIT-FOR-SERVER",argv[0],(pointer (*)())EUSTF_TFBC_WAITFORSERVER);
00865   defun(ctx,"EUSTF-TF2-CAN-TRANSFORM",argv[0],(pointer (*)())EUSTF_TFBC_CANTRANSFORM);
00866   defun(ctx,"EUSTF-TF2-LOOKUP-TRANSFORM",argv[0],(pointer (*)())EUSTF_TFBC_LOOKUPTRANSFORM);
00867 
00868   pointer_update(Spevalof(PACKAGE),p);
00869   return 0;
00870 }


roseus
Author(s): Kei Okada
autogenerated on Wed Sep 16 2015 10:23:58