00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047 #include "pcdfilter_pa/pcdfilter_pa_ros_filter.h"
00048
00049
00050 #include <sstream>
00051
00052
00053
00054
00055
00056
00057 cPcdFilterPaRosFilter::cPcdFilterPaRosFilter(void) {
00058
00059 reset();
00060 }
00061
00062
00063 cPcdFilterPaRosFilter::cPcdFilterPaRosFilter(
00064 const cPcdFilterPaRosFilter &other) {
00065
00066 *this = other;
00067 }
00068
00069
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
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
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
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
00315 bool cPcdFilterPaRosFilter::isValid() const {
00316
00317 return true;
00318 }
00319
00320
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
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
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
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
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
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
00428
00429
00430
00431 cPcdFilterPaRosFilters::cPcdFilterPaRosFilters(void) {
00432
00433 tf_lookup_time_ = 0.2;
00434 }
00435
00436
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
00451 void cPcdFilterPaRosFilters::clear() {
00452
00453 filters_.clear();
00454 }
00455
00456
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
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
00475 std::vector<std::pair<std::string, tf::Transform > > frames;
00476
00477 ros::Time start_time = ros::Time::now();
00478
00479 for (int i_filter = 0; i_filter < filters_.size(); i_filter++) {
00480
00481
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
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
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
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
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
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
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
00562 if (skip_filter) {
00563 if (filters_[i_filter].required_) {
00564 ROS_DEBUG("%s",error_msg.data());
00565 }
00566
00567
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
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
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
00672 std::string cPcdFilterPaRosFilters::getLast() const {
00673
00674 return last_filter_.toString();
00675 }
00676
00677
00678 int cPcdFilterPaRosFilters::size() const {
00679 return filters_.size();
00680 }
00681
00682
00683 cPcdFilterPaRosFilter& cPcdFilterPaRosFilters::operator[](int i) {
00684 return filters_[i];
00685 }
00686
00687
00688 const cPcdFilterPaRosFilter& cPcdFilterPaRosFilters::operator[](int i) const {
00689 return filters_[i];
00690 }
00691
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