pcdfilter_pa_ros_filter.cpp
Go to the documentation of this file.
00001 /******************************************************************************
00002 *                                                                             *
00003 * pcdfilter_pa_ros_filter.cpp                                                 *
00004 * ===========================                                                 *
00005 *                                                                             *
00006 *******************************************************************************
00007 *                                                                             *
00008 * github repository                                                           *
00009 *   https://github.com/peterweissig/ros_pcdfilter                             *
00010 *                                                                             *
00011 * Chair of Automation Technology, Technische Universität Chemnitz             *
00012 *   https://www.tu-chemnitz.de/etit/proaut                                    *
00013 *                                                                             *
00014 *******************************************************************************
00015 *                                                                             *
00016 * New BSD License                                                             *
00017 *                                                                             *
00018 * Copyright (c) 2015-2017, Peter Weissig, Technische Universität Chemnitz     *
00019 * All rights reserved.                                                        *
00020 *                                                                             *
00021 * Redistribution and use in source and binary forms, with or without          *
00022 * modification, are permitted provided that the following conditions are met: *
00023 *     * Redistributions of source code must retain the above copyright        *
00024 *       notice, this list of conditions and the following disclaimer.         *
00025 *     * Redistributions in binary form must reproduce the above copyright     *
00026 *       notice, this list of conditions and the following disclaimer in the   *
00027 *       documentation and/or other materials provided with the distribution.  *
00028 *     * Neither the name of the Technische Universität Chemnitz nor the       *
00029 *       names of its contributors may be used to endorse or promote products  *
00030 *       derived from this software without specific prior written permission. *
00031 *                                                                             *
00032 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" *
00033 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE   *
00034 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE  *
00035 * ARE DISCLAIMED. IN NO EVENT SHALL <COPYRIGHT HOLDER> BE LIABLE FOR ANY      *
00036 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES  *
00037 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR          *
00038 * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER  *
00039 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT          *
00040 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY   *
00041 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH *
00042 * DAMAGE.                                                                     *
00043 *                                                                             *
00044 ******************************************************************************/
00045 
00046 // local headers
00047 #include "pcdfilter_pa/pcdfilter_pa_ros_filter.h"
00048 
00049 // standard headers
00050 #include <sstream>
00051 
00052 //********************************************************************************
00053 //**************************[cPcdFilterPaRosFilter]*******************************
00054 //********************************************************************************
00055 
00056 //**************************[cPcdFilterPaRosFilter]*******************************
00057 cPcdFilterPaRosFilter::cPcdFilterPaRosFilter(void) {
00058 
00059     reset();
00060 }
00061 
00062 //**************************[cPcdFilterPaRosFilter]*******************************
00063 cPcdFilterPaRosFilter::cPcdFilterPaRosFilter(
00064   const cPcdFilterPaRosFilter &other) {
00065 
00066     *this = other;
00067 }
00068 
00069 //**************************[operator = ]**************************************
00070 cPcdFilterPaRosFilter& cPcdFilterPaRosFilter::operator = (
00071   const cPcdFilterPaRosFilter &other) {
00072 
00073     inverse_          = other.inverse_     ;
00074     type_             = other.type_        ;
00075     required_         = other.required_    ;
00076 
00077     for (int i = 0; i < COUNT_PARAMETER; i++) {
00078         parameter_[i] = other.parameter_[i];
00079     }
00080 
00081     for (int i = 0; i < COUNT_FRAME; i++) {
00082         frame_[i]     = other.frame_[i]    ;
00083         offset_[i]    = other.offset_[i]   ;
00084     }
00085 
00086     comment_          = other.comment_     ;
00087 
00088     return *this;
00089 }
00090 
00091 
00092 //**************************[fromString]***************************************
00093 bool cPcdFilterPaRosFilter::fromString(const std::string &filter) {
00094 
00095     reset();
00096 
00097     int pos = 0;
00098     std::string s;
00099 
00100     _skipWhitespace(filter, pos);
00101     inverse_ = _checkSymbol(filter,pos,'!');
00102 
00103     _skipWhitespace(filter,pos);
00104     s = _getValue(filter, pos);
00105     if (s == "cube") {
00106         type_ = ftCUBE;
00107     } else if (s == "sphere") {
00108         type_ = ftSPHERE;
00109     } else if (s == "block") {
00110         type_ = ftBLOCK;
00111     } else if (s == "cylinder") {
00112         type_ = ftCYLINDER;
00113     } else if (s == "link") {
00114         type_ = ftLINK;
00115     } else if (s == "cone") {
00116         type_ = ftCONE;
00117     } else {
00118         return false;
00119     }
00120 
00121     _skipWhitespace(filter,pos);
00122     if (_checkSymbol(filter,pos,':')) {
00123         required_ = true;
00124     } else if (_checkSymbol(filter,pos,'?')) {
00125         required_ = false;
00126     } else {
00127         return false;
00128     }
00129 
00130 
00131     for (int i = 0; ; i++) {
00132         _skipWhitespace(filter,pos);
00133         s = _getValue(filter, pos);
00134         if (s == "") {
00135             return false;
00136         }
00137         if (i >= COUNT_PARAMETER) { break;}
00138         if (! _StrToFloat(s, parameter_[i])) { break;}
00139     }
00140 
00141     for (int i = 0; i < COUNT_FRAME; i++) {
00142         double d;
00143         if (_StrToFloat(s, d)) {
00144             return false;
00145         }
00146         frame_[i] = s;
00147         _skipWhitespace(filter, pos);
00148         s = _getValue(filter, pos);
00149 
00150         tf::Quaternion q = tf::Quaternion::getIdentity();
00151         for (int j = 0; ; j++) {
00152             if (! _StrToFloat(s, d)) {
00153                 if (j > 3) {
00154                     if (j < 7) {
00155                         q.setW(0);
00156                     }
00157                     q.normalize();
00158                     offset_[i].setRotation(q);
00159                 }
00160                 offset_[i] = offset_[i].inverse();
00161                 break;
00162             }
00163             if (j >= 7) {
00164                 q.normalize();
00165                 offset_[i].setRotation(q);
00166                 offset_[i] = offset_[i].inverse();
00167                 return false;
00168             } else {
00169                 _skipWhitespace(filter, pos);
00170                 s = _getValue(filter, pos);
00171             }
00172             switch (j) {
00173                 case 0: offset_[i].getOrigin().setX(d); break;
00174                 case 1: offset_[i].getOrigin().setY(d); break;
00175                 case 2: offset_[i].getOrigin().setZ(d); break;
00176 
00177                 case 3: q.setX(d); break;
00178                 case 4: q.setY(d); break;
00179                 case 5: q.setZ(d); break;
00180                 case 6: q.setW(d); break;
00181             }
00182         }
00183         if (s == "") {
00184             break;
00185         }
00186     }
00187 
00188     comment_ = _getComment(filter);
00189 
00190     return isValid();
00191 }
00192 
00193 //**************************[toString]*****************************************
00194 std::string cPcdFilterPaRosFilter::toString() const {
00195 
00196     std::stringstream result;
00197 
00198     if (inverse_) {result << '!';}
00199 
00200     int param_count = 1;
00201     int tf_count    = 1;
00202     switch (type_) {
00203         case ftCUBE:
00204             result << "cube";
00205             break;
00206         case ftSPHERE:
00207             result << "sphere";
00208             break;
00209         case ftBLOCK:
00210             result << "block";
00211             param_count = 3;
00212             break;
00213         case ftCYLINDER:
00214             result << "cylinder";
00215             param_count = 2;
00216             break;
00217         case ftLINK:
00218             result << "link";
00219             param_count = 2;
00220             tf_count = 2;
00221             break;
00222         case ftCONE:
00223             result << "cone";
00224             param_count = 2;
00225             break;
00226         case ftNONE:
00227             result << "-";
00228             param_count = 0;
00229             tf_count = 0;
00230             break;
00231         default:
00232             result << "error";
00233             param_count = 0;
00234             tf_count = 0;
00235             break;
00236     }
00237 
00238     if (required_) {
00239         result << ':';
00240     } else {
00241         result << "?";
00242     }
00243 
00244     if (param_count > COUNT_PARAMETER) { param_count = COUNT_PARAMETER;}
00245     for (int i = 0; i < param_count; i++) {
00246         result << ' ' << parameter_[i];
00247     }
00248 
00249     if (tf_count > COUNT_FRAME) { tf_count = COUNT_FRAME;}
00250     for (int i = 0; i < tf_count; i++) {
00251         tf::Transform temp_offset = offset_[i].inverse();
00252         result << ' ' << frame_[i];
00253 
00254         bool _x = false;
00255         bool _y = false;
00256         bool _z = false;
00257         bool _q = false;
00258         if (temp_offset.getOrigin().x() != 0) {
00259             _x = true;
00260         }
00261         if (temp_offset.getOrigin().y() != 0) {
00262             _x = _y = true;
00263         }
00264         if (temp_offset.getOrigin().z() != 0) {
00265             _x = _y = _z = true;
00266         }
00267         if ((temp_offset.getRotation().x() != 0) ||
00268           (temp_offset.getRotation().y() != 0) ||
00269           (temp_offset.getRotation().z() != 0) ||
00270           (temp_offset.getRotation().w() != 1)) {
00271             _x = _y = _z = _q = true;
00272         }
00273 
00274         if (_x) {
00275             result << ' ' << _floatToStr(temp_offset.getOrigin().x());
00276         }
00277         if (_y) {
00278             result << ' ' << _floatToStr(temp_offset.getOrigin().y());
00279         }
00280         if (_z) {
00281             result << ' ' << _floatToStr(temp_offset.getOrigin().z());
00282         }
00283         if (_q) {
00284             result << ' ' << _floatToStr(temp_offset.getRotation().x());
00285             result << ' ' << _floatToStr(temp_offset.getRotation().y());
00286             result << ' ' << _floatToStr(temp_offset.getRotation().z());
00287             result << ' ' << _floatToStr(temp_offset.getRotation().w());
00288         }
00289     }
00290 
00291     if (comment_ != "") {
00292         result << " #" << comment_;
00293     }
00294 
00295     return result.str();
00296 }
00297 
00298 //**************************[reset]********************************************
00299 void cPcdFilterPaRosFilter::reset() {
00300 
00301     inverse_  = false;
00302     type_ = ftNONE;
00303     required_ = true;
00304     for (int i = 0; i < COUNT_FRAME; i++) {
00305         frame_[i] = "";
00306         offset_[i].setIdentity();
00307     }
00308     for (int i = 0; i < COUNT_PARAMETER; i++) {
00309         parameter_[i] = 0;
00310     }
00311     comment_ = "";
00312 }
00313 
00314 //**************************[isValid]******************************************
00315 bool cPcdFilterPaRosFilter::isValid() const {
00316 
00317     return true;
00318 }
00319 
00320 //**************************[_skipWhitespace]**********************************
00321 bool cPcdFilterPaRosFilter::_skipWhitespace(const std::string &str, int &pos)
00322   const {
00323 
00324     if (pos >= str.size()) {
00325         return false;
00326     }
00327 
00328     char c = str[pos];
00329     if ((c != ' ') && (c != '\t')) {
00330         return false;
00331     }
00332 
00333     while (1) {
00334         pos++;
00335         if (pos >= str.size()) {
00336             return false;
00337         }
00338 
00339         char c = str[pos];
00340         if ((c != ' ') && (c != '\t')) {
00341             break;
00342         }
00343     }
00344 
00345     return true;
00346 }
00347 
00348 //**************************[_checkSymbol]*************************************
00349 bool cPcdFilterPaRosFilter::_checkSymbol(const std::string &str, int &pos,
00350   const char symbol) const {
00351 
00352     if (pos >= str.size()) {
00353         return false;
00354     }
00355 
00356     if (str[pos] == symbol) {
00357         pos++;
00358         return true;
00359     }
00360 
00361     return false;
00362 }
00363 
00364 //**************************[_getValue]****************************************
00365 std::string cPcdFilterPaRosFilter::_getValue(const std::string &str, int &pos)
00366   const {
00367 
00368     int start = pos;
00369 
00370     while (1) {
00371         if (pos >= str.size()) {
00372             break;
00373         }
00374 
00375         char c = str[pos];
00376         if ((c == ' ') || (c == '\t') || (c == '#') || (c == ':') ||
00377           (c == '?')) {
00378             break;
00379         }
00380         pos++;
00381     }
00382 
00383     if (pos <= start) {
00384         return "";
00385     } else {
00386         return str.substr(start, pos - start);
00387     }
00388 }
00389 
00390 //**************************[_getComment]**************************************
00391 std::string cPcdFilterPaRosFilter::_getComment(const std::string &str)
00392   const {
00393 
00394     int pos = (int)str.find('#');
00395     if (pos <= 0) {
00396         return "";
00397     }
00398 
00399     return str.substr(pos + 1);
00400 }
00401 
00402 //**************************[_floatToStr]**************************************
00403 std::string cPcdFilterPaRosFilter::_floatToStr(const double &value) const {
00404 
00405     std::stringstream ss;
00406 
00407     ss << value;
00408     return ss.str();
00409 }
00410 
00411 //**************************[_StrToFloat]**************************************
00412 bool cPcdFilterPaRosFilter::_StrToFloat(const std::string &str,
00413     double &value) const {
00414 
00415     std::stringstream ss;
00416 
00417     ss << str;
00418     try {
00419         ss >> value;
00420     } catch (std::runtime_error &e) {
00421         return false;
00422     }
00423     return ! ss.fail();
00424 }
00425 
00426 //*****************************************************************************
00427 //**************************[cPcdFilterPaRosFilters]***************************
00428 //*****************************************************************************
00429 
00430 //**************************[cPcdFilterPaRosFilters]***************************
00431 cPcdFilterPaRosFilters::cPcdFilterPaRosFilters(void) {
00432 
00433     tf_lookup_time_  = 0.2;
00434 }
00435 
00436 //**************************[add]**********************************************
00437 bool cPcdFilterPaRosFilters::add(std::string filter) {
00438 
00439     cPcdFilterPaRosFilter f;
00440 
00441     bool result = f.fromString(filter);
00442     last_filter_ = f;
00443     if (result) {
00444         filters_.push_back(f);
00445     }
00446 
00447     return result;
00448 }
00449 
00450 //**************************[clear]********************************************
00451 void cPcdFilterPaRosFilters::clear() {
00452 
00453     filters_.clear();
00454 }
00455 
00456 //**************************[get]**********************************************
00457 std::vector<std::string> cPcdFilterPaRosFilters::get() const {
00458 
00459     std::vector<std::string> result;
00460     for (int i = 0; i < filters_.size(); i++) {
00461         result.push_back(filters_[i].toString());
00462     }
00463     return result;
00464 }
00465 
00466 //**************************[update]*******************************************
00467 bool cPcdFilterPaRosFilters::update(
00468   const tf::TransformListener &tf_listener,
00469   const std::string base_frame, const ros::Time time,
00470   std::vector<cPcdFilterPaFilter> &result) const {
00471 
00472     result.clear();
00473 
00474     // buffer for frame_id/tf pairs (to increase speed)
00475     std::vector<std::pair<std::string, tf::Transform > > frames;
00476 
00477     ros::Time start_time = ros::Time::now();
00478     // iterate over all filters
00479     for (int i_filter = 0; i_filter < filters_.size(); i_filter++) {
00480 
00481         // check number of tfs (always 1 - except for "ftLink")
00482         int tf_count = 0;
00483         switch (filters_[i_filter].type_) {
00484             case cPcdFilterPaRosFilter::ftCUBE    :
00485             case cPcdFilterPaRosFilter::ftSPHERE  :
00486             case cPcdFilterPaRosFilter::ftBLOCK   :
00487             case cPcdFilterPaRosFilter::ftCYLINDER:
00488                 tf_count = 1;
00489                 break;
00490             case cPcdFilterPaRosFilter::ftLINK    :
00491                 tf_count = 2;
00492                 break;
00493             case cPcdFilterPaRosFilter::ftCONE    :
00494                 tf_count = 2;
00495                 break;
00496             case cPcdFilterPaRosFilter::ftNONE    :
00497                 continue;
00498             default                               :
00499                 return false;
00500         }
00501 
00502         // lookup tf based on frame id
00503         bool skip_filter = false;
00504         tf::Transform tf_temp[cPcdFilterPaRosFilter::COUNT_FRAME];
00505         for (int i_tf = 0; i_tf < tf_count; i_tf++) {
00506             std::string frame_temp = filters_[i_filter].frame_[i_tf];
00507 
00508             // check buffer first
00509             int i_frames = 0;
00510             for (; i_frames < frames.size(); i_frames++) {
00511                 if (frames[i_frames].first == frame_temp) {
00512                     tf_temp[i_tf] = frames[i_frames].second;
00513 
00514                     if (tf_temp[i_tf].getBasis()[0][0] < -2) {
00515                         skip_filter = true;
00516                     }
00517                     break;
00518                 }
00519             }
00520 
00521             // look up is necessary
00522             if (i_frames >= frames.size()) {
00523                 tf::StampedTransform stf_temp;
00524                 std::string error_msg;
00525                 try {
00526                     bool check_transform = true;
00527 
00528                     // wait for transform
00529                     if (tf_lookup_time_ > 0) {
00530                         ros::Duration current_duration = ros::Time::now() - start_time;
00531                         if (current_duration < ros::Duration(tf_lookup_time_)) {
00532                             ros::Duration max_duration = ros::Duration(tf_lookup_time_) - current_duration;
00533                             if (tf_listener.waitForTransform(frame_temp,
00534                               base_frame, time, max_duration,
00535                               ros::Duration(0.01), &error_msg)) {
00536                                 check_transform = false;
00537                             } else {
00538                                 skip_filter = true;
00539                             }
00540                         }
00541                     }
00542 
00543                     // check if transform is possible
00544                     if ((! skip_filter) && check_transform) {
00545                         if(! tf_listener.canTransform(frame_temp,
00546                           base_frame, time, &error_msg)) {
00547                             skip_filter = true;
00548                         }
00549                     }
00550 
00551                     // get transform
00552                     if (! skip_filter) {
00553                         tf_listener.lookupTransform(frame_temp, base_frame,
00554                           time, stf_temp);
00555                     }
00556                 } catch (tf::TransformException &ex){
00557                     skip_filter = true;
00558                     error_msg = ex.what();
00559                 }
00560 
00561                 // error handling
00562                 if (skip_filter) {
00563                     if (filters_[i_filter].required_) {
00564                         ROS_DEBUG("%s",error_msg.data());
00565                     }
00566 
00567                     // save a invalid tf (rot[0,0] < -2)
00568                     tf_temp[i_tf].getBasis()[0][0] = -10;
00569                     frames.push_back(std::pair<std::string, tf::Transform>
00570                       (frame_temp, tf_temp[i_tf]));
00571                 } else {
00572                     // save current tf
00573                     tf_temp[i_tf] = stf_temp;
00574                     frames.push_back(std::pair<std::string, tf::Transform>
00575                       (frame_temp, tf_temp[i_tf]));
00576                 }
00577             }
00578 
00579             if (skip_filter) {
00580                 if (filters_[i_filter].required_) {
00581                     return false;
00582                 }
00583                 break;
00584             }
00585 
00586             tf_temp[i_tf] = filters_[i_filter].offset_[i_tf] * tf_temp[i_tf];
00587         }
00588 
00589         // create final filters
00590         cPcdFilterPaFilter filter_temp;
00591         if (skip_filter) {
00592             filter_temp.type_ = cPcdFilterPaFilter::ftNONE;
00593             result.push_back(filter_temp);
00594             continue;
00595         }
00596 
00597         switch (filters_[i_filter].type_) {
00598             case cPcdFilterPaRosFilter::ftCUBE    :
00599                 filter_temp.type_ = cPcdFilterPaFilter::ftCUBE;
00600                 filter_temp.parameter_[0] =
00601                   filters_[i_filter].parameter_[0] / 2;
00602                 break;
00603             case cPcdFilterPaRosFilter::ftSPHERE  :
00604                 filter_temp.type_ = cPcdFilterPaFilter::ftSPHERE;
00605                 filter_temp.parameter_[0] = filters_[i_filter].parameter_[0] *
00606                   filters_[i_filter].parameter_[0];
00607                 break;
00608             case cPcdFilterPaRosFilter::ftBLOCK   :
00609                 filter_temp.type_ = cPcdFilterPaFilter::ftBLOCK;
00610                 for (int i = 0; i < 3; i++) {
00611                     filter_temp.parameter_[i] =
00612                       filters_[i_filter].parameter_[i] / 2;
00613                 }
00614                 break;
00615             case cPcdFilterPaRosFilter::ftCYLINDER:
00616                 filter_temp.type_ = cPcdFilterPaFilter::ftCYLINDER;
00617                 filter_temp.parameter_[0] = filters_[i_filter].parameter_[0] *
00618                   filters_[i_filter].parameter_[0];
00619                 filter_temp.parameter_[1] =
00620                   filters_[i_filter].parameter_[1] / 2;
00621                 break;
00622             case cPcdFilterPaRosFilter::ftLINK    :
00623                 {
00624                                         filter_temp.type_ = cPcdFilterPaFilter::ftCYLINDER;
00625 
00626                                         double d = update_transform(tf_temp[0], tf_temp[1],
00627                                           tf_temp[0], 0.5);
00628                                         if (d < 0) {
00629                                                 filter_temp.type_ = cPcdFilterPaFilter::ftNONE;
00630                                                 break;
00631                                         }
00632 
00633                                         filter_temp.parameter_[0] =
00634                                           filters_[i_filter].parameter_[0] *
00635                                           filters_[i_filter].parameter_[0];
00636                                         filter_temp.parameter_[1] = d / 2 +
00637                                           filters_[i_filter].parameter_[1];
00638                 }
00639 
00640                 break;
00641             case cPcdFilterPaRosFilter::ftCONE    :
00642                 filter_temp.type_ = cPcdFilterPaFilter::ftCONE;
00643 
00644                 if (update_transform(tf_temp[0], tf_temp[1], tf_temp[0], 0.5) < 0) {
00645                     filter_temp.type_ = cPcdFilterPaFilter::ftNONE;
00646                     break;
00647                 }
00648 
00649                 filter_temp.parameter_[0] = filters_[i_filter].parameter_[0];
00650                 filter_temp.parameter_[1] = filters_[i_filter].parameter_[1] * filters_[i_filter].parameter_[1];
00651                 break;
00652         }
00653 
00654         filter_temp.inverse_ = filters_[i_filter].inverse_;
00655         filter_temp.translation_[0] = tf_temp[0].getOrigin().x();
00656         filter_temp.translation_[1] = tf_temp[0].getOrigin().y();
00657         filter_temp.translation_[2] = tf_temp[0].getOrigin().z();
00658 
00659         for (int y = 0; y < 3; y++) {
00660             for (int x = 0; x < 3; x++) {
00661                 filter_temp.rotation_(y,x) = tf_temp[0].getBasis()[y][x];
00662             }
00663         }
00664 
00665         result.push_back(filter_temp);
00666     }
00667 
00668     return true;
00669 }
00670 
00671 //**************************[getLast]******************************************
00672 std::string cPcdFilterPaRosFilters::getLast() const {
00673 
00674     return last_filter_.toString();
00675 }
00676 
00677 //**************************[size]*********************************************
00678 int cPcdFilterPaRosFilters::size() const {
00679     return filters_.size();
00680 }
00681 
00682 //**************************[operator]*****************************************
00683 cPcdFilterPaRosFilter& cPcdFilterPaRosFilters::operator[](int i) {
00684     return filters_[i];
00685 }
00686 
00687 //**************************[operator]*****************************************
00688 const cPcdFilterPaRosFilter& cPcdFilterPaRosFilters::operator[](int i) const {
00689     return filters_[i];
00690 }
00691 //**************************[operator]*****************************************
00692 double cPcdFilterPaRosFilters::update_transform(const tf::Transform &start,
00693   const tf::Transform &goal, tf::Transform &result, const double relative_pos) const {
00694 
00695     tf::Vector3 v = (start * goal.inverse()).getOrigin();
00696     double d = v.length();
00697 
00698     if (v.fuzzyZero()) {
00699         return -1;
00700     }
00701     tf::Vector3 axis = v;
00702     v.normalize();
00703     axis = tf::Vector3 (1,0,0).cross(v);
00704 
00705     tf::Transform tf_shift;
00706     if (! axis.fuzzyZero()) {
00707         tf::Quaternion q(axis.normalized(),tfAsin(axis.length()));
00708         q = q.inverse();
00709         tf_shift.setRotation(q);
00710     } else {
00711         tf_shift.setRotation(tf::Quaternion().getIdentity());
00712     }
00713     tf_shift.setOrigin(tf::Vector3(d * relative_pos,0,0));
00714 
00715     result = tf_shift * start;
00716     return d;
00717 }
00718 


pcdfilter_pa
Author(s):
autogenerated on Thu Aug 3 2017 03:00:01